diff options
Diffstat (limited to 'source/blender/blenkernel/intern/collision.c')
| -rw-r--r-- | source/blender/blenkernel/intern/collision.c | 434 |
1 files changed, 432 insertions, 2 deletions
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c index 3030ee4b4a4..b38609e3a31 100644 --- a/source/blender/blenkernel/intern/collision.c +++ b/source/blender/blenkernel/intern/collision.c @@ -652,8 +652,8 @@ static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, Collis *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 +913,433 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa return 1|MIN2 ( ret, 1 ); } + + +static int cloth_points_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end) +{ + int result = 0; + Cloth *cloth1; + float w1, w2, u1, u2, u3; + float v1[3], v2[3], relativeVelocity[3]; + float magrelVel; + float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); + + cloth1 = clmd->clothObject; + + for ( ; collpair != collision_end; collpair++ ) { +// float i1[3], i2[3], i3[3]; + +// zero_v3(i1); +// zero_v3(i2); +// zero_v3(i3); + + /* 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, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 ); + + sub_v3_v3v3(relativeVelocity, v2, v1); + + /* Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). */ + magrelVel = dot_v3v3(relativeVelocity, collpair->normal); + + /* printf("magrelVel: %f\n", magrelVel); */ + + /* Calculate masses of points. + * TODO */ + +#if 0 + /* If v_n_mag < 0 the edges are approaching each other. */ + if ( magrelVel > ALMOST_ZERO ) { + /* Calculate Impulse magnitude to stop all motion in normal direction. */ + float magtangent = 0, repulse = 0, d = 0; + double impulse = 0.0; + float vrel_t_pre[3]; + float temp[3], spf; + + /* calculate tangential velocity */ + copy_v3_v3 ( temp, collpair->normal ); + mul_v3_fl(temp, magrelVel); + sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp); + + /* Decrease in magnitude of relative tangential velocity due to coulomb friction + * in original formula "magrelVel" should be the "change of relative velocity in normal direction" */ + magtangent = min_ff(clmd->coll_parms->friction * 0.01f * magrelVel, len_v3(vrel_t_pre)); + + /* Apply friction impulse. */ + if ( magtangent > ALMOST_ZERO ) { + normalize_v3(vrel_t_pre); + + impulse = magtangent / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* 2.0 * */ + VECADDMUL ( i1, vrel_t_pre, w1 * impulse ); + VECADDMUL ( i2, vrel_t_pre, w2 * impulse ); + VECADDMUL ( i3, vrel_t_pre, w3 * impulse ); + } + + /* Apply velocity stopping impulse + * I_c = m * v_N / 2.0 + * no 2.0 * magrelVel normally, but looks nicer DG */ + impulse = magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); + + VECADDMUL ( i1, collpair->normal, w1 * impulse ); + cloth1->verts[collpair->ap1].impulse_count++; + + VECADDMUL ( i2, collpair->normal, w2 * impulse ); + cloth1->verts[collpair->ap2].impulse_count++; + + VECADDMUL ( i3, collpair->normal, w3 * impulse ); + cloth1->verts[collpair->ap3].impulse_count++; + + /* Apply repulse impulse if distance too short + * I_r = -min(dt*kd, m(0, 1d/dt - v_n)) + * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this: + * v += impulse; x_new = x + v; + * We don't use dt!! + * DG TODO: Fix usage of dt here! */ + spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; + + d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance; + if ( ( magrelVel < 0.1f*d*spf ) && ( d > ALMOST_ZERO ) ) { + repulse = MIN2 ( d*1.0f/spf, 0.1f*d*spf - magrelVel ); + + /* stay on the safe side and clamp repulse */ + if ( impulse > ALMOST_ZERO ) + repulse = min_ff( repulse, 5.0*impulse ); + repulse = max_ff(impulse, repulse); + + impulse = repulse / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* original 2.0 / 0.25 */ + VECADDMUL ( i1, collpair->normal, impulse ); + VECADDMUL ( i2, collpair->normal, impulse ); + VECADDMUL ( i3, collpair->normal, impulse ); + } + + result = 1; + } + else { + /* Apply repulse impulse if distance too short + * I_r = -min(dt*kd, max(0, 1d/dt - v_n)) + * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this: + * v += impulse; x_new = x + v; + * We don't use dt!! */ + float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; + + float d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - (float)collpair->distance; + if ( d > ALMOST_ZERO) { + /* stay on the safe side and clamp repulse */ + float repulse = d*1.0f/spf; + + float impulse = repulse / ( 3.0f * ( 1.0f + w1*w1 + w2*w2 + w3*w3 )); /* original 2.0 / 0.25 */ + + VECADDMUL ( i1, collpair->normal, impulse ); + VECADDMUL ( i2, collpair->normal, impulse ); + VECADDMUL ( i3, collpair->normal, impulse ); + + cloth1->verts[collpair->ap1].impulse_count++; + cloth1->verts[collpair->ap2].impulse_count++; + cloth1->verts[collpair->ap3].impulse_count++; + + result = 1; + } + } + + if (result) { + int i = 0; + + for (i = 0; i < 3; i++) { + if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[i]) < ABS(i1[i])) + cloth1->verts[collpair->ap1].impulse[i] = i1[i]; + + if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[i]) < ABS(i2[i])) + cloth1->verts[collpair->ap2].impulse[i] = i2[i]; + + if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[i]) < ABS(i3[i])) + cloth1->verts[collpair->ap3].impulse[i] = i3[i]; + } + } +#endif + } + return result; +} + +BLI_INLINE void face_normal(float co1[3], float co2[3], float co3[3], float nor[3]) +{ + float p[3], q[3]; + sub_v3_v3v3(p, co2, co1); + sub_v3_v3v3(q, co3, co1); + cross_v3_v3v3(nor, p, q); + normalize_v3(nor); +} + +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, CollPair *collpair) +{ + float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co; + float lambda, uv[2]; + float fnor[3], vec[3]; + float w[3]; + + if (!isect_line_tri_v3(p1, p2, co1, co2, co3, &lambda, uv)) + return collpair; + + face_normal(co1, co2, co3, fnor); + sub_v3_v3v3(vec, p2, p1); + + 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; + + copy_v3_v3(collpair->pa, p1); + w[0] = 1.0f - uv[0] - uv[1]; + w[1] = uv[0]; + w[2] = uv[1]; + interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w); + + /* note: face normal smoothing is ignored here, + * it would probably not work with collision response + */ + copy_v3_v3(collpair->normal, fnor); + collpair->distance = -dot_v3v3(fnor, vec) * (1.0f - lambda); + copy_v3_v3(collpair->vector, vec); + 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, 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->xnew; + + vert = &clmd->clothObject->verts[overlap->indexA]; + face = &collmd->mfaces[overlap->indexB]; + + collpair = cloth_point_collpair(vert->x, vert->tx, mverts, face->v1, face->v2, face->v3, overlap->indexA, overlap->indexB, collpair); + if (face->v4) + collpair = cloth_point_collpair(vert->x, vert->tx, mverts, face->v3, face->v4, face->v1, overlap->indexA, overlap->indexB, collpair); + + return collpair; +} + +static void cloth_points_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd, + CollPair **collisions, CollPair **collisions_index, + int numresult, BVHTreeOverlap *overlap, 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, *collisions_index, dt); + } +} + +static int cloth_points_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index) +{ + Cloth *cloth = clmd->clothObject; + int i=0, numverts = clmd->clothObject->numverts; + ClothVertex *verts = cloth->verts; + int ret = 0; + + // process all collisions + if ( collmd->bvhtree ) { + int result = cloth_points_collision_response_static(clmd, collmd, collisions, collisions_index); + + // 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++; + } + } + } + } +#if 0 + // process all collisions (calculate impulses, TODO: also repulses if distance too short) + result = 1; + for ( j = 0; j < 2; j++ ) { /* 5 is just a value that ensures convergence */ + result = 0; + + if ( collmd->bvhtree ) { + result += cloth_points_collision_response_static(clmd, collmd, collisions, collisions_index); + + // 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++; + } + } + } + } + + if (!result) { + break; + } + } +#endif + + 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), 2, 26); + /* 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; + + if (!collmd->bvhtree) + continue; + + /* search for overlapping collision pairs */ + overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result ); + + // 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, round_dt); + + // resolve nearby collisions + ret += cloth_points_objcollisions_resolve(clmd, collmd, collisions[i], collisions_index[i]); + 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 ); +} |