diff options
Diffstat (limited to 'source/blender/blenkernel/intern/collision.c')
-rw-r--r-- | source/blender/blenkernel/intern/collision.c | 2853 |
1 files changed, 1498 insertions, 1355 deletions
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c index a7ba143fc9d..67e1f36b222 100644 --- a/source/blender/blenkernel/intern/collision.c +++ b/source/blender/blenkernel/intern/collision.c @@ -21,7 +21,6 @@ * \ingroup bke */ - #include "MEM_guardedalloc.h" #include "DNA_cloth_types.h" @@ -53,25 +52,24 @@ #include "DEG_depsgraph_query.h" #ifdef WITH_ELTOPO -#include "eltopo-capi.h" +# include "eltopo-capi.h" #endif - typedef struct ColDetectData { - ClothModifierData *clmd; - CollisionModifierData *collmd; - BVHTreeOverlap *overlap; - CollPair *collisions; - bool culling; - bool use_normal; - bool collided; + ClothModifierData *clmd; + CollisionModifierData *collmd; + BVHTreeOverlap *overlap; + CollPair *collisions; + bool culling; + bool use_normal; + bool collided; } ColDetectData; typedef struct SelfColDetectData { - ClothModifierData *clmd; - BVHTreeOverlap *overlap; - CollPair *collisions; - bool collided; + ClothModifierData *clmd; + BVHTreeOverlap *overlap; + CollPair *collisions; + bool collided; } SelfColDetectData; /*********************************** @@ -81,103 +79,103 @@ Collision modifier code start /* step is limited from 0 (frame start position) to 1 (frame end position) */ void collision_move_object(CollisionModifierData *collmd, float step, float prevstep) { - float oldx[3]; - unsigned int i = 0; - - /* the collider doesn't move this frame */ - if (collmd->is_static) { - for (i = 0; i < collmd->mvert_num; i++) { - zero_v3(collmd->current_v[i].co); - } - - return; - } - - for (i = 0; i < collmd->mvert_num; i++) { - interp_v3_v3v3(oldx, collmd->x[i].co, collmd->xnew[i].co, prevstep); - interp_v3_v3v3(collmd->current_x[i].co, collmd->x[i].co, collmd->xnew[i].co, step); - sub_v3_v3v3(collmd->current_v[i].co, collmd->current_x[i].co, oldx); - } - - bvhtree_update_from_mvert( - collmd->bvhtree, collmd->current_x, NULL, - collmd->tri, collmd->tri_num, false); + float oldx[3]; + unsigned int i = 0; + + /* the collider doesn't move this frame */ + if (collmd->is_static) { + for (i = 0; i < collmd->mvert_num; i++) { + zero_v3(collmd->current_v[i].co); + } + + return; + } + + for (i = 0; i < collmd->mvert_num; i++) { + interp_v3_v3v3(oldx, collmd->x[i].co, collmd->xnew[i].co, prevstep); + interp_v3_v3v3(collmd->current_x[i].co, collmd->x[i].co, collmd->xnew[i].co, step); + sub_v3_v3v3(collmd->current_v[i].co, collmd->current_x[i].co, oldx); + } + + bvhtree_update_from_mvert( + collmd->bvhtree, collmd->current_x, NULL, collmd->tri, collmd->tri_num, false); } -BVHTree *bvhtree_build_from_mvert( - const MVert *mvert, - const struct MVertTri *tri, int tri_num, - float epsilon) +BVHTree *bvhtree_build_from_mvert(const MVert *mvert, + const struct MVertTri *tri, + int tri_num, + float epsilon) { - BVHTree *tree; - const MVertTri *vt; - int i; + BVHTree *tree; + const MVertTri *vt; + int i; - tree = BLI_bvhtree_new(tri_num, epsilon, 4, 26); + tree = BLI_bvhtree_new(tri_num, epsilon, 4, 26); - /* fill tree */ - for (i = 0, vt = tri; i < tri_num; i++, vt++) { - float co[3][3]; + /* fill tree */ + for (i = 0, vt = tri; i < tri_num; i++, vt++) { + float co[3][3]; - 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); + 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[0], 3); - } + BLI_bvhtree_insert(tree, i, co[0], 3); + } - /* balance tree */ - BLI_bvhtree_balance(tree); + /* balance tree */ + BLI_bvhtree_balance(tree); - return tree; + return tree; } -void bvhtree_update_from_mvert( - BVHTree *bvhtree, - const MVert *mvert, const MVert *mvert_moving, - const MVertTri *tri, int tri_num, - bool 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; - - if ((bvhtree == NULL) || (mvert == NULL)) { - return; - } - - 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); - - /* 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); - } - - /* check if tree is already full */ - if (ret == false) { - break; - } - } - - BLI_bvhtree_update_tree(bvhtree); + const MVertTri *vt; + int i; + + if ((bvhtree == NULL) || (mvert == NULL)) { + return; + } + + 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); + + /* 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); + } + + /* check if tree is already full */ + if (ret == false) { + break; + } + } + + BLI_bvhtree_update_tree(bvhtree); } /*********************************** @@ -186,268 +184,277 @@ Collision modifier code end BLI_INLINE int next_ind(int i) { - return (++i < 3) ? i : 0; + return (++i < 3) ? i : 0; } -static float compute_collision_point(float a1[3], float a2[3], float a3[3], float b1[3], float b2[3], float b3[3], - bool culling, bool use_normal, float r_a[3], float r_b[3], float r_vec[3]) +static float compute_collision_point(float a1[3], + float a2[3], + float a3[3], + float b1[3], + float b2[3], + float b3[3], + bool culling, + bool use_normal, + float r_a[3], + float r_b[3], + float r_vec[3]) { - float a[3][3]; - float b[3][3]; - float dist = FLT_MAX; - float tmp_co1[3], tmp_co2[3]; - float isect_a[3], isect_b[3]; - int isect_count = 0; - float tmp, tmp_vec[3]; - float normal[3], cent[3]; - bool backside = false; - - copy_v3_v3(a[0], a1); - copy_v3_v3(a[1], a2); - copy_v3_v3(a[2], a3); - - copy_v3_v3(b[0], b1); - copy_v3_v3(b[1], b2); - copy_v3_v3(b[2], b3); - - /* Find intersections. */ - for (int i = 0; i < 3; i++) { - if (isect_line_segment_tri_v3(a[i], a[next_ind(i)], b[0], b[1], b[2], &tmp, NULL)) { - interp_v3_v3v3(isect_a, a[i], a[next_ind(i)], tmp); - isect_count++; - } - } - - if (isect_count == 0) { - for (int i = 0; i < 3; i++) { - if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) { - isect_count++; - } - } - } - else if (isect_count == 1) { - for (int i = 0; i < 3; i++) { - if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) { - interp_v3_v3v3(isect_b, b[i], b[next_ind(i)], tmp); - break; - } - } - } - - /* Determine collision side. */ - if (culling) { - normal_tri_v3(normal, b[0], b[1], b[2]); - mid_v3_v3v3v3(cent, b[0], b[1], b[2]); - - if (isect_count == 2) { - backside = true; - } - else if (isect_count == 0) { - for (int i = 0; i < 3; i++) { - sub_v3_v3v3(tmp_vec, a[i], cent); - if (dot_v3v3(tmp_vec, normal) < 0.0f) { - backside = true; - break; - } - } - } - } - else if (use_normal) { - normal_tri_v3(normal, b[0], b[1], b[2]); - } - - if (isect_count == 1) { - /* Edge intersection. */ - copy_v3_v3(r_a, isect_a); - copy_v3_v3(r_b, isect_b); - - if (use_normal) { - copy_v3_v3(r_vec, normal); - } - else { - sub_v3_v3v3(r_vec, r_b, r_a); - } - - return 0.0f; - } - - if (backside) { - float maxdist = 0.0f; - bool found = false; - - /* Point projections. */ - for (int i = 0; i < 3; i++) { - if (isect_ray_tri_v3(a[i], normal, b[0], b[1], b[2], &tmp, NULL)) { - if (tmp > maxdist) { - maxdist = tmp; - copy_v3_v3(r_a, a[i]); - madd_v3_v3v3fl(r_b, a[i], normal, tmp); - found = true; - } - } - } - - negate_v3(normal); - - for (int i = 0; i < 3; i++) { - if (isect_ray_tri_v3(b[i], normal, a[0], a[1], a[2], &tmp, NULL)) { - if (tmp > maxdist) { - maxdist = tmp; - madd_v3_v3v3fl(r_a, b[i], normal, tmp); - copy_v3_v3(r_b, b[i]); - found = true; - } - } - } - - negate_v3(normal); - - /* Edge projections. */ - for (int i = 0; i < 3; i++) { - float dir[3]; - - sub_v3_v3v3(tmp_vec, b[next_ind(i)], b[i]); - cross_v3_v3v3(dir, tmp_vec, normal); - - for (int j = 0; j < 3; j++) { - if (isect_line_plane_v3(tmp_co1, a[j], a[next_ind(j)], b[i], dir) && - point_in_slice_seg(tmp_co1, a[j], a[next_ind(j)]) && - point_in_slice_seg(tmp_co1, b[i], b[next_ind(i)])) - { - closest_to_line_v3(tmp_co2, tmp_co1, b[i], b[next_ind(i)]); - sub_v3_v3v3(tmp_vec, tmp_co1, tmp_co2); - tmp = len_v3(tmp_vec); - - if ((tmp > maxdist) && (dot_v3v3(tmp_vec, normal) < 0.0f)) { - maxdist = tmp; - copy_v3_v3(r_a, tmp_co1); - copy_v3_v3(r_b, tmp_co2); - found = true; - } - } - } - } - - /* If no point is found, will fallback onto regular proximity test below. */ - if (found) { - sub_v3_v3v3(r_vec, r_b, r_a); - - if (use_normal) { - if (dot_v3v3(normal, r_vec) >= 0.0f) { - copy_v3_v3(r_vec, normal); - } - else { - negate_v3_v3(r_vec, normal); - } - } - - return 0.0f; - } - } - - /* Closest point. */ - for (int i = 0; i < 3; i++) { - closest_on_tri_to_point_v3(tmp_co1, a[i], b[0], b[1], b[2]); - tmp = len_squared_v3v3(tmp_co1, a[i]); - - if (tmp < dist) { - dist = tmp; - copy_v3_v3(r_a, a[i]); - copy_v3_v3(r_b, tmp_co1); - } - } - - for (int i = 0; i < 3; i++) { - closest_on_tri_to_point_v3(tmp_co1, b[i], a[0], a[1], a[2]); - tmp = len_squared_v3v3(tmp_co1, b[i]); - - if (tmp < dist) { - dist = tmp; - copy_v3_v3(r_a, tmp_co1); - copy_v3_v3(r_b, b[i]); - } - } - - /* Closest edge. */ - if (isect_count == 0) { - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - isect_seg_seg_v3(a[i], a[next_ind(i)], b[j], b[next_ind(j)], tmp_co1, tmp_co2); - tmp = len_squared_v3v3(tmp_co1, tmp_co2); - - if (tmp < dist) { - dist = tmp; - copy_v3_v3(r_a, tmp_co1); - copy_v3_v3(r_b, tmp_co2); - } - } - } - } - - if (isect_count == 0) { - sub_v3_v3v3(r_vec, r_a, r_b); - dist = sqrtf(dist); - } - else { - sub_v3_v3v3(r_vec, r_b, r_a); - dist = 0.0f; - } - - if (culling && use_normal) { - copy_v3_v3(r_vec, normal); - } - else if (use_normal) { - if (dot_v3v3(normal, r_vec) >= 0.0f) { - copy_v3_v3(r_vec, normal); - } - else { - negate_v3_v3(r_vec, normal); - } - } - else if (culling && (dot_v3v3(r_vec, normal) < 0.0f)) { - return FLT_MAX; - } - - return dist; + float a[3][3]; + float b[3][3]; + float dist = FLT_MAX; + float tmp_co1[3], tmp_co2[3]; + float isect_a[3], isect_b[3]; + int isect_count = 0; + float tmp, tmp_vec[3]; + float normal[3], cent[3]; + bool backside = false; + + copy_v3_v3(a[0], a1); + copy_v3_v3(a[1], a2); + copy_v3_v3(a[2], a3); + + copy_v3_v3(b[0], b1); + copy_v3_v3(b[1], b2); + copy_v3_v3(b[2], b3); + + /* Find intersections. */ + for (int i = 0; i < 3; i++) { + if (isect_line_segment_tri_v3(a[i], a[next_ind(i)], b[0], b[1], b[2], &tmp, NULL)) { + interp_v3_v3v3(isect_a, a[i], a[next_ind(i)], tmp); + isect_count++; + } + } + + if (isect_count == 0) { + for (int i = 0; i < 3; i++) { + if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) { + isect_count++; + } + } + } + else if (isect_count == 1) { + for (int i = 0; i < 3; i++) { + if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) { + interp_v3_v3v3(isect_b, b[i], b[next_ind(i)], tmp); + break; + } + } + } + + /* Determine collision side. */ + if (culling) { + normal_tri_v3(normal, b[0], b[1], b[2]); + mid_v3_v3v3v3(cent, b[0], b[1], b[2]); + + if (isect_count == 2) { + backside = true; + } + else if (isect_count == 0) { + for (int i = 0; i < 3; i++) { + sub_v3_v3v3(tmp_vec, a[i], cent); + if (dot_v3v3(tmp_vec, normal) < 0.0f) { + backside = true; + break; + } + } + } + } + else if (use_normal) { + normal_tri_v3(normal, b[0], b[1], b[2]); + } + + if (isect_count == 1) { + /* Edge intersection. */ + copy_v3_v3(r_a, isect_a); + copy_v3_v3(r_b, isect_b); + + if (use_normal) { + copy_v3_v3(r_vec, normal); + } + else { + sub_v3_v3v3(r_vec, r_b, r_a); + } + + return 0.0f; + } + + if (backside) { + float maxdist = 0.0f; + bool found = false; + + /* Point projections. */ + for (int i = 0; i < 3; i++) { + if (isect_ray_tri_v3(a[i], normal, b[0], b[1], b[2], &tmp, NULL)) { + if (tmp > maxdist) { + maxdist = tmp; + copy_v3_v3(r_a, a[i]); + madd_v3_v3v3fl(r_b, a[i], normal, tmp); + found = true; + } + } + } + + negate_v3(normal); + + for (int i = 0; i < 3; i++) { + if (isect_ray_tri_v3(b[i], normal, a[0], a[1], a[2], &tmp, NULL)) { + if (tmp > maxdist) { + maxdist = tmp; + madd_v3_v3v3fl(r_a, b[i], normal, tmp); + copy_v3_v3(r_b, b[i]); + found = true; + } + } + } + + negate_v3(normal); + + /* Edge projections. */ + for (int i = 0; i < 3; i++) { + float dir[3]; + + sub_v3_v3v3(tmp_vec, b[next_ind(i)], b[i]); + cross_v3_v3v3(dir, tmp_vec, normal); + + for (int j = 0; j < 3; j++) { + if (isect_line_plane_v3(tmp_co1, a[j], a[next_ind(j)], b[i], dir) && + point_in_slice_seg(tmp_co1, a[j], a[next_ind(j)]) && + point_in_slice_seg(tmp_co1, b[i], b[next_ind(i)])) { + closest_to_line_v3(tmp_co2, tmp_co1, b[i], b[next_ind(i)]); + sub_v3_v3v3(tmp_vec, tmp_co1, tmp_co2); + tmp = len_v3(tmp_vec); + + if ((tmp > maxdist) && (dot_v3v3(tmp_vec, normal) < 0.0f)) { + maxdist = tmp; + copy_v3_v3(r_a, tmp_co1); + copy_v3_v3(r_b, tmp_co2); + found = true; + } + } + } + } + + /* If no point is found, will fallback onto regular proximity test below. */ + if (found) { + sub_v3_v3v3(r_vec, r_b, r_a); + + if (use_normal) { + if (dot_v3v3(normal, r_vec) >= 0.0f) { + copy_v3_v3(r_vec, normal); + } + else { + negate_v3_v3(r_vec, normal); + } + } + + return 0.0f; + } + } + + /* Closest point. */ + for (int i = 0; i < 3; i++) { + closest_on_tri_to_point_v3(tmp_co1, a[i], b[0], b[1], b[2]); + tmp = len_squared_v3v3(tmp_co1, a[i]); + + if (tmp < dist) { + dist = tmp; + copy_v3_v3(r_a, a[i]); + copy_v3_v3(r_b, tmp_co1); + } + } + + for (int i = 0; i < 3; i++) { + closest_on_tri_to_point_v3(tmp_co1, b[i], a[0], a[1], a[2]); + tmp = len_squared_v3v3(tmp_co1, b[i]); + + if (tmp < dist) { + dist = tmp; + copy_v3_v3(r_a, tmp_co1); + copy_v3_v3(r_b, b[i]); + } + } + + /* Closest edge. */ + if (isect_count == 0) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + isect_seg_seg_v3(a[i], a[next_ind(i)], b[j], b[next_ind(j)], tmp_co1, tmp_co2); + tmp = len_squared_v3v3(tmp_co1, tmp_co2); + + if (tmp < dist) { + dist = tmp; + copy_v3_v3(r_a, tmp_co1); + copy_v3_v3(r_b, tmp_co2); + } + } + } + } + + if (isect_count == 0) { + sub_v3_v3v3(r_vec, r_a, r_b); + dist = sqrtf(dist); + } + else { + sub_v3_v3v3(r_vec, r_b, r_a); + dist = 0.0f; + } + + if (culling && use_normal) { + copy_v3_v3(r_vec, normal); + } + else if (use_normal) { + if (dot_v3v3(normal, r_vec) >= 0.0f) { + copy_v3_v3(r_vec, normal); + } + else { + negate_v3_v3(r_vec, normal); + } + } + else if (culling && (dot_v3v3(r_vec, normal) < 0.0f)) { + return FLT_MAX; + } + + return dist; } // w3 is not perfect -static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 ) +static void collision_compute_barycentric( + float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3) { - /* dot_v3v3 */ -#define INPR(v1, v2) ( (v1)[0] * (v2)[0] + (v1)[1] * (v2)[1] + (v1)[2] * (v2)[2]) + /* dot_v3v3 */ +#define INPR(v1, v2) ((v1)[0] * (v2)[0] + (v1)[1] * (v2)[1] + (v1)[2] * (v2)[2]) - double tempV1[3], tempV2[3], tempV4[3]; - double a, b, c, d, e, f; + double tempV1[3], tempV2[3], tempV4[3]; + double a, b, c, d, e, f; - sub_v3db_v3fl_v3fl(tempV1, p1, p3); - sub_v3db_v3fl_v3fl(tempV2, p2, p3); - sub_v3db_v3fl_v3fl(tempV4, pv, p3); + sub_v3db_v3fl_v3fl(tempV1, p1, p3); + sub_v3db_v3fl_v3fl(tempV2, p2, p3); + sub_v3db_v3fl_v3fl(tempV4, pv, p3); - a = INPR ( tempV1, tempV1 ); - b = INPR ( tempV1, tempV2 ); - c = INPR ( tempV2, tempV2 ); - e = INPR ( tempV1, tempV4 ); - f = INPR ( tempV2, tempV4 ); + a = INPR(tempV1, tempV1); + b = INPR(tempV1, tempV2); + c = INPR(tempV2, tempV2); + e = INPR(tempV1, tempV4); + f = INPR(tempV2, tempV4); - d = ( a * c - b * b ); + d = (a * c - b * b); - if ( ABS ( d ) < (double)ALMOST_ZERO ) { - *w1 = *w2 = *w3 = 1.0 / 3.0; - return; - } + if (ABS(d) < (double)ALMOST_ZERO) { + *w1 = *w2 = *w3 = 1.0 / 3.0; + return; + } - w1[0] = ( float ) ( ( e * c - b * f ) / d ); + w1[0] = (float)((e * c - b * f) / d); - if ( w1[0] < 0 ) - w1[0] = 0; + if (w1[0] < 0) + w1[0] = 0; - w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c ); + w2[0] = (float)((f - b * (double)w1[0]) / c); - if ( w2[0] < 0 ) - w2[0] = 0; + if (w2[0] < 0) + w2[0] = 0; - w3[0] = 1.0f - w1[0] - w2[0]; + w3[0] = 1.0f - w1[0] - w2[0]; #undef INPR } @@ -457,1131 +464,1267 @@ static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3 # pragma GCC diagnostic ignored "-Wdouble-promotion" #endif -DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 ) +DO_INLINE void collision_interpolateOnTriangle( + float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3) { - zero_v3(to); - VECADDMUL(to, v1, w1); - VECADDMUL(to, v2, w2); - VECADDMUL(to, v3, w3); + zero_v3(to); + VECADDMUL(to, v1, w1); + VECADDMUL(to, v2, w2); + VECADDMUL(to, v3, w3); } -static int cloth_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, Object *collob, - CollPair *collpair, uint collision_count, const float dt) +static int cloth_collision_response_static(ClothModifierData *clmd, + CollisionModifierData *collmd, + Object *collob, + CollPair *collpair, + uint collision_count, + const float dt) { - int result = 0; - Cloth *cloth1; - float w1, w2, w3, u1, u2, u3; - float v1[3], v2[3], relativeVelocity[3]; - float magrelVel; - float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree); - - cloth1 = clmd->clothObject; - - for (int i = 0; i < collision_count; i++, 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 | COLLISION_INACTIVE)) { - continue; - } - - /* Compute barycentric coordinates for both collision points. */ - collision_compute_barycentric(collpair->pa, - cloth1->verts[collpair->ap1].tx, - cloth1->verts[collpair->ap2].tx, - cloth1->verts[collpair->ap3].tx, - &w1, &w2, &w3); - - 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". */ - collision_interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3); - - 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); - - /* If magrelVel < 0 the edges are approaching each other. */ - if (magrelVel > 0.0f) { - /* 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]; - float time_multiplier; - - /* 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(collob->pd->pdef_cfrict * 0.01f * magrelVel, len_v3(vrel_t_pre)); - - /* Apply friction impulse. */ - if ( magtangent > ALMOST_ZERO ) { - normalize_v3(vrel_t_pre); - - impulse = magtangent / 1.5; - - 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. */ - impulse = magrelVel / 1.5f; - - 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++; - - time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); + int result = 0; + Cloth *cloth1; + float w1, w2, w3, u1, u2, u3; + float v1[3], v2[3], relativeVelocity[3]; + float magrelVel; + float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree); + + cloth1 = clmd->clothObject; + + for (int i = 0; i < collision_count; i++, 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 | COLLISION_INACTIVE)) { + continue; + } + + /* Compute barycentric coordinates for both collision points. */ + collision_compute_barycentric(collpair->pa, + cloth1->verts[collpair->ap1].tx, + cloth1->verts[collpair->ap2].tx, + cloth1->verts[collpair->ap3].tx, + &w1, + &w2, + &w3); + + 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". */ + collision_interpolateOnTriangle(v1, + cloth1->verts[collpair->ap1].tv, + cloth1->verts[collpair->ap2].tv, + cloth1->verts[collpair->ap3].tv, + w1, + w2, + w3); + + 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); + + /* If magrelVel < 0 the edges are approaching each other. */ + if (magrelVel > 0.0f) { + /* 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]; + float time_multiplier; + + /* 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(collob->pd->pdef_cfrict * 0.01f * magrelVel, len_v3(vrel_t_pre)); + + /* Apply friction impulse. */ + if (magtangent > ALMOST_ZERO) { + normalize_v3(vrel_t_pre); + + impulse = magtangent / 1.5; + + 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. */ + impulse = magrelVel / 1.5f; + + 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++; + + time_multiplier = 1.0f / (clmd->sim_parms->dt * 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 * time_multiplier) && (d > ALMOST_ZERO)) { + repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel); + + /* Stay on the safe side and clamp repulse. */ + if (impulse > ALMOST_ZERO) { + repulse = min_ff(repulse, 5.0f * impulse); + } + + repulse = max_ff(impulse, repulse); + + impulse = repulse / 1.5f; + + VECADDMUL(i1, collpair->normal, impulse); + VECADDMUL(i2, collpair->normal, impulse); + VECADDMUL(i3, collpair->normal, impulse); + } + + result = 1; + } + else { + float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); + float d; + + d = clmd->coll_parms->epsilon * 8.0f / 9.0f + epsilon2 * 8.0f / 9.0f - collpair->distance; + + if (d > ALMOST_ZERO) { + /* Stay on the safe side and clamp repulse. */ + float repulse = d / time_multiplier; + float impulse = repulse / 4.5f; + + VECADDMUL(i1, collpair->normal, w1 * impulse); + VECADDMUL(i2, collpair->normal, w2 * impulse); + VECADDMUL(i3, collpair->normal, w3 * impulse); + + cloth1->verts[collpair->ap1].impulse_count++; + cloth1->verts[collpair->ap2].impulse_count++; + cloth1->verts[collpair->ap3].impulse_count++; - d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance; + result = 1; + } + } - if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) { - repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel); + if (result) { + float clamp = clmd->coll_parms->clamp * dt; - /* Stay on the safe side and clamp repulse. */ - if (impulse > ALMOST_ZERO) { - repulse = min_ff(repulse, 5.0f * impulse); - } + if ((clamp > 0.0f) && + ((len_v3(i1) > clamp) || (len_v3(i2) > clamp) || (len_v3(i3) > clamp))) { + return 0; + } - repulse = max_ff(impulse, repulse); + for (int j = 0; j < 3; j++) { + if (cloth1->verts[collpair->ap1].impulse_count > 0 && + ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j])) + cloth1->verts[collpair->ap1].impulse[j] = i1[j]; - impulse = repulse / 1.5f; + if (cloth1->verts[collpair->ap2].impulse_count > 0 && + ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j])) + cloth1->verts[collpair->ap2].impulse[j] = i2[j]; - VECADDMUL(i1, collpair->normal, impulse); - VECADDMUL(i2, collpair->normal, impulse); - VECADDMUL(i3, collpair->normal, impulse); - } + if (cloth1->verts[collpair->ap3].impulse_count > 0 && + ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j])) + cloth1->verts[collpair->ap3].impulse[j] = i3[j]; + } + } + } - result = 1; - } - else { - float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); - float d; - - d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance; - - if (d > ALMOST_ZERO) { - /* Stay on the safe side and clamp repulse. */ - float repulse = d / time_multiplier; - float impulse = repulse / 4.5f; - - VECADDMUL(i1, collpair->normal, w1 * impulse); - VECADDMUL(i2, collpair->normal, w2 * impulse); - VECADDMUL(i3, collpair->normal, w3 * impulse); - - cloth1->verts[collpair->ap1].impulse_count++; - cloth1->verts[collpair->ap2].impulse_count++; - cloth1->verts[collpair->ap3].impulse_count++; - - result = 1; - } - } - - if (result) { - float clamp = clmd->coll_parms->clamp * dt; - - if ((clamp > 0.0f) && - ((len_v3(i1) > clamp) || - (len_v3(i2) > clamp) || - (len_v3(i3) > clamp))) - { - return 0; - } - - for (int j = 0; j < 3; j++) { - if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j])) - cloth1->verts[collpair->ap1].impulse[j] = i1[j]; - - if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j])) - cloth1->verts[collpair->ap2].impulse[j] = i2[j]; - - if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j])) - cloth1->verts[collpair->ap3].impulse[j] = i3[j]; - } - } - } - - return result; + return result; } -static int cloth_selfcollision_response_static(ClothModifierData *clmd, CollPair *collpair, - uint collision_count, const float dt) +static int cloth_selfcollision_response_static(ClothModifierData *clmd, + CollPair *collpair, + uint collision_count, + const float dt) { - int result = 0; - Cloth *cloth1; - float w1, w2, w3, u1, u2, u3; - float v1[3], v2[3], relativeVelocity[3]; - float magrelVel; - - cloth1 = clmd->clothObject; - - for (int i = 0; i < collision_count; i++, 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 | COLLISION_INACTIVE)) { - continue; - } - - /* Compute barycentric coordinates for both collision points. */ - collision_compute_barycentric(collpair->pa, - cloth1->verts[collpair->ap1].tx, - cloth1->verts[collpair->ap2].tx, - cloth1->verts[collpair->ap3].tx, - &w1, &w2, &w3); - - collision_compute_barycentric(collpair->pb, - cloth1->verts[collpair->bp1].tx, - cloth1->verts[collpair->bp2].tx, - cloth1->verts[collpair->bp3].tx, - &u1, &u2, &u3); - - /* Calculate relative "velocity". */ - collision_interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3); - - collision_interpolateOnTriangle(v2, cloth1->verts[collpair->bp1].tv, cloth1->verts[collpair->bp2].tv, cloth1->verts[collpair->bp3].tv, 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); - - /* TODO: Impulses should be weighed by mass as this is self col, - * this has to be done after mass distribution is implemented. */ - - /* If magrelVel < 0 the edges are approaching each other. */ - if (magrelVel > 0.0f) { - /* 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], time_multiplier; + int result = 0; + Cloth *cloth1; + float w1, w2, w3, u1, u2, u3; + float v1[3], v2[3], relativeVelocity[3]; + float magrelVel; + + cloth1 = clmd->clothObject; + + for (int i = 0; i < collision_count; i++, 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 | COLLISION_INACTIVE)) { + continue; + } + + /* Compute barycentric coordinates for both collision points. */ + collision_compute_barycentric(collpair->pa, + cloth1->verts[collpair->ap1].tx, + cloth1->verts[collpair->ap2].tx, + cloth1->verts[collpair->ap3].tx, + &w1, + &w2, + &w3); + + collision_compute_barycentric(collpair->pb, + cloth1->verts[collpair->bp1].tx, + cloth1->verts[collpair->bp2].tx, + cloth1->verts[collpair->bp3].tx, + &u1, + &u2, + &u3); + + /* Calculate relative "velocity". */ + collision_interpolateOnTriangle(v1, + cloth1->verts[collpair->ap1].tv, + cloth1->verts[collpair->ap2].tv, + cloth1->verts[collpair->ap3].tv, + w1, + w2, + w3); + + collision_interpolateOnTriangle(v2, + cloth1->verts[collpair->bp1].tv, + cloth1->verts[collpair->bp2].tv, + cloth1->verts[collpair->bp3].tv, + 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); + + /* TODO: Impulses should be weighed by mass as this is self col, + * this has to be done after mass distribution is implemented. */ + + /* If magrelVel < 0 the edges are approaching each other. */ + if (magrelVel > 0.0f) { + /* 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], time_multiplier; + + /* 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->self_friction * 0.01f * magrelVel, len_v3(vrel_t_pre)); + + /* Apply friction impulse. */ + if (magtangent > ALMOST_ZERO) { + normalize_v3(vrel_t_pre); + + impulse = magtangent / 1.5; + + 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. */ + impulse = magrelVel / 3.0f; + + 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++; + + time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); + + d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance; + + if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) { + repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel); + + if (impulse > ALMOST_ZERO) { + repulse = min_ff(repulse, 5.0 * impulse); + } + + repulse = max_ff(impulse, repulse); + + impulse = repulse / 1.5f; + + VECADDMUL(i1, collpair->normal, w1 * impulse); + VECADDMUL(i2, collpair->normal, w2 * impulse); + VECADDMUL(i3, collpair->normal, w3 * impulse); + } + + result = 1; + } + else { + float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); + float d; + + d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance; + + if (d > ALMOST_ZERO) { + /* Stay on the safe side and clamp repulse. */ + float repulse = d * 1.0f / time_multiplier; + float impulse = repulse / 9.0f; - /* Calculate tangential velocity. */ - copy_v3_v3(temp, collpair->normal); - mul_v3_fl(temp, magrelVel); - sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp); + VECADDMUL(i1, collpair->normal, w1 * impulse); + VECADDMUL(i2, collpair->normal, w2 * impulse); + VECADDMUL(i3, collpair->normal, w3 * impulse); - /* 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->self_friction * 0.01f * magrelVel, len_v3(vrel_t_pre)); + cloth1->verts[collpair->ap1].impulse_count++; + cloth1->verts[collpair->ap2].impulse_count++; + cloth1->verts[collpair->ap3].impulse_count++; - /* Apply friction impulse. */ - if (magtangent > ALMOST_ZERO) { - normalize_v3(vrel_t_pre); + result = 1; + } + } - impulse = magtangent / 1.5; + if (result) { + float clamp = clmd->coll_parms->self_clamp * dt; - VECADDMUL(i1, vrel_t_pre, w1 * impulse); - VECADDMUL(i2, vrel_t_pre, w2 * impulse); - VECADDMUL(i3, vrel_t_pre, w3 * impulse); - } + if ((clamp > 0.0f) && + ((len_v3(i1) > clamp) || (len_v3(i2) > clamp) || (len_v3(i3) > clamp))) { + return 0; + } - /* Apply velocity stopping impulse. */ - impulse = magrelVel / 3.0f; + for (int j = 0; j < 3; j++) { + if (cloth1->verts[collpair->ap1].impulse_count > 0 && + ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j])) + cloth1->verts[collpair->ap1].impulse[j] = i1[j]; - VECADDMUL(i1, collpair->normal, w1 * impulse); - cloth1->verts[collpair->ap1].impulse_count++; + if (cloth1->verts[collpair->ap2].impulse_count > 0 && + ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j])) + cloth1->verts[collpair->ap2].impulse[j] = i2[j]; - VECADDMUL(i2, collpair->normal, w2 * impulse); - cloth1->verts[collpair->ap2].impulse_count++; + if (cloth1->verts[collpair->ap3].impulse_count > 0 && + ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j])) + cloth1->verts[collpair->ap3].impulse[j] = i3[j]; + } + } + } - VECADDMUL(i3, collpair->normal, w3 * impulse); - cloth1->verts[collpair->ap3].impulse_count++; - - time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); - - d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance; - - if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) { - repulse = MIN2 (d / time_multiplier, 0.1f * d * time_multiplier - magrelVel); - - if (impulse > ALMOST_ZERO) { - repulse = min_ff(repulse, 5.0*impulse); - } - - repulse = max_ff(impulse, repulse); - - impulse = repulse / 1.5f; - - VECADDMUL(i1, collpair->normal, w1 * impulse); - VECADDMUL(i2, collpair->normal, w2 * impulse); - VECADDMUL(i3, collpair->normal, w3 * impulse); - } - - result = 1; - } - else { - float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale); - float d; - - d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance; - - if ( d > ALMOST_ZERO) { - /* Stay on the safe side and clamp repulse. */ - float repulse = d*1.0f/time_multiplier; - float impulse = repulse / 9.0f; - - VECADDMUL(i1, collpair->normal, w1 * impulse); - VECADDMUL(i2, collpair->normal, w2 * impulse); - VECADDMUL(i3, collpair->normal, w3 * impulse); - - cloth1->verts[collpair->ap1].impulse_count++; - cloth1->verts[collpair->ap2].impulse_count++; - cloth1->verts[collpair->ap3].impulse_count++; - - result = 1; - } - } - - if (result) { - float clamp = clmd->coll_parms->self_clamp * dt; - - if ((clamp > 0.0f) && - ((len_v3(i1) > clamp) || - (len_v3(i2) > clamp) || - (len_v3(i3) > clamp))) - { - return 0; - } - - for (int j = 0; j < 3; j++) { - if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j])) - cloth1->verts[collpair->ap1].impulse[j] = i1[j]; - - if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j])) - cloth1->verts[collpair->ap2].impulse[j] = i2[j]; - - if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j])) - cloth1->verts[collpair->ap3].impulse[j] = i3[j]; - } - } - } - - return result; + return result; } #ifdef __GNUC__ # pragma GCC diagnostic pop #endif -static void cloth_collision( - void *__restrict userdata, - const int index, - const ParallelRangeTLS *__restrict UNUSED(tls)) +static void cloth_collision(void *__restrict userdata, + const int index, + const ParallelRangeTLS *__restrict UNUSED(tls)) { - ColDetectData *data = (ColDetectData *)userdata; - - ClothModifierData *clmd = data->clmd; - CollisionModifierData *collmd = data->collmd; - CollPair *collpair = data->collisions; - const MVertTri *tri_a, *tri_b; - ClothVertex *verts1 = clmd->clothObject->verts; - float distance = 0.0f; - float epsilon1 = clmd->coll_parms->epsilon; - float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree); - float pa[3], pb[3], vect[3]; - - tri_a = &clmd->clothObject->tri[data->overlap[index].indexA]; - tri_b = &collmd->tri[data->overlap[index].indexB]; - - /* Compute distance and normal. */ - distance = compute_collision_point(verts1[tri_a->tri[0]].tx, verts1[tri_a->tri[1]].tx, verts1[tri_a->tri[2]].tx, - collmd->current_x[tri_b->tri[0]].co, collmd->current_x[tri_b->tri[1]].co, collmd->current_x[tri_b->tri[2]].co, - data->culling, data->use_normal, pa, pb, vect); - - if ((distance <= (epsilon1 + epsilon2 + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) { - collpair[index].ap1 = tri_a->tri[0]; - collpair[index].ap2 = tri_a->tri[1]; - collpair[index].ap3 = tri_a->tri[2]; - - collpair[index].bp1 = tri_b->tri[0]; - collpair[index].bp2 = tri_b->tri[1]; - collpair[index].bp3 = tri_b->tri[2]; - - copy_v3_v3(collpair[index].pa, pa); - copy_v3_v3(collpair[index].pb, pb); - copy_v3_v3(collpair[index].vector, vect); - - normalize_v3_v3(collpair[index].normal, collpair[index].vector); - - collpair[index].distance = distance; - collpair[index].flag = 0; - - data->collided = true; - } - else { - collpair[index].flag = COLLISION_INACTIVE; - } + ColDetectData *data = (ColDetectData *)userdata; + + ClothModifierData *clmd = data->clmd; + CollisionModifierData *collmd = data->collmd; + CollPair *collpair = data->collisions; + const MVertTri *tri_a, *tri_b; + ClothVertex *verts1 = clmd->clothObject->verts; + float distance = 0.0f; + float epsilon1 = clmd->coll_parms->epsilon; + float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree); + float pa[3], pb[3], vect[3]; + + tri_a = &clmd->clothObject->tri[data->overlap[index].indexA]; + tri_b = &collmd->tri[data->overlap[index].indexB]; + + /* Compute distance and normal. */ + distance = compute_collision_point(verts1[tri_a->tri[0]].tx, + verts1[tri_a->tri[1]].tx, + verts1[tri_a->tri[2]].tx, + collmd->current_x[tri_b->tri[0]].co, + collmd->current_x[tri_b->tri[1]].co, + collmd->current_x[tri_b->tri[2]].co, + data->culling, + data->use_normal, + pa, + pb, + vect); + + if ((distance <= (epsilon1 + epsilon2 + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) { + collpair[index].ap1 = tri_a->tri[0]; + collpair[index].ap2 = tri_a->tri[1]; + collpair[index].ap3 = tri_a->tri[2]; + + collpair[index].bp1 = tri_b->tri[0]; + collpair[index].bp2 = tri_b->tri[1]; + collpair[index].bp3 = tri_b->tri[2]; + + copy_v3_v3(collpair[index].pa, pa); + copy_v3_v3(collpair[index].pb, pb); + copy_v3_v3(collpair[index].vector, vect); + + normalize_v3_v3(collpair[index].normal, collpair[index].vector); + + collpair[index].distance = distance; + collpair[index].flag = 0; + + data->collided = true; + } + else { + collpair[index].flag = COLLISION_INACTIVE; + } } -static void cloth_selfcollision( - void *__restrict userdata, - const int index, - const ParallelRangeTLS *__restrict UNUSED(tls)) +static void cloth_selfcollision(void *__restrict userdata, + const int index, + const ParallelRangeTLS *__restrict UNUSED(tls)) { - SelfColDetectData *data = (SelfColDetectData *)userdata; - - ClothModifierData *clmd = data->clmd; - CollPair *collpair = data->collisions; - const MVertTri *tri_a, *tri_b; - ClothVertex *verts1 = clmd->clothObject->verts; - float distance = 0.0f; - float epsilon = clmd->coll_parms->selfepsilon; - float pa[3], pb[3], vect[3]; - - tri_a = &clmd->clothObject->tri[data->overlap[index].indexA]; - tri_b = &clmd->clothObject->tri[data->overlap[index].indexB]; - - for (uint i = 0; i < 3; i++) { - for (uint j = 0; j < 3; j++) { - if (tri_a->tri[i] == tri_b->tri[j]) { - collpair[index].flag = COLLISION_INACTIVE; - return; - } - } - } - - if (((verts1[tri_a->tri[0]].flags & verts1[tri_a->tri[1]].flags & verts1[tri_a->tri[2]].flags) | - (verts1[tri_b->tri[0]].flags & verts1[tri_b->tri[1]].flags & verts1[tri_b->tri[2]].flags)) & CLOTH_VERT_FLAG_NOSELFCOLL) - { - collpair[index].flag = COLLISION_INACTIVE; - return; - } - - /* Compute distance and normal. */ - distance = compute_collision_point(verts1[tri_a->tri[0]].tx, verts1[tri_a->tri[1]].tx, verts1[tri_a->tri[2]].tx, - verts1[tri_b->tri[0]].tx, verts1[tri_b->tri[1]].tx, verts1[tri_b->tri[2]].tx, - false, false, pa, pb, vect); - - if ((distance <= (epsilon * 2.0f + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) { - collpair[index].ap1 = tri_a->tri[0]; - collpair[index].ap2 = tri_a->tri[1]; - collpair[index].ap3 = tri_a->tri[2]; - - collpair[index].bp1 = tri_b->tri[0]; - collpair[index].bp2 = tri_b->tri[1]; - collpair[index].bp3 = tri_b->tri[2]; - - copy_v3_v3(collpair[index].pa, pa); - copy_v3_v3(collpair[index].pb, pb); - copy_v3_v3(collpair[index].vector, vect); - - normalize_v3_v3(collpair[index].normal, collpair[index].vector); - - collpair[index].distance = distance; - collpair[index].flag = 0; - - data->collided = true; - } - else { - collpair[index].flag = COLLISION_INACTIVE; - } + SelfColDetectData *data = (SelfColDetectData *)userdata; + + ClothModifierData *clmd = data->clmd; + CollPair *collpair = data->collisions; + const MVertTri *tri_a, *tri_b; + ClothVertex *verts1 = clmd->clothObject->verts; + float distance = 0.0f; + float epsilon = clmd->coll_parms->selfepsilon; + float pa[3], pb[3], vect[3]; + + tri_a = &clmd->clothObject->tri[data->overlap[index].indexA]; + tri_b = &clmd->clothObject->tri[data->overlap[index].indexB]; + + for (uint i = 0; i < 3; i++) { + for (uint j = 0; j < 3; j++) { + if (tri_a->tri[i] == tri_b->tri[j]) { + collpair[index].flag = COLLISION_INACTIVE; + return; + } + } + } + + if (((verts1[tri_a->tri[0]].flags & verts1[tri_a->tri[1]].flags & verts1[tri_a->tri[2]].flags) | + (verts1[tri_b->tri[0]].flags & verts1[tri_b->tri[1]].flags & verts1[tri_b->tri[2]].flags)) & + CLOTH_VERT_FLAG_NOSELFCOLL) { + collpair[index].flag = COLLISION_INACTIVE; + return; + } + + /* Compute distance and normal. */ + distance = compute_collision_point(verts1[tri_a->tri[0]].tx, + verts1[tri_a->tri[1]].tx, + verts1[tri_a->tri[2]].tx, + verts1[tri_b->tri[0]].tx, + verts1[tri_b->tri[1]].tx, + verts1[tri_b->tri[2]].tx, + false, + false, + pa, + pb, + vect); + + if ((distance <= (epsilon * 2.0f + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) { + collpair[index].ap1 = tri_a->tri[0]; + collpair[index].ap2 = tri_a->tri[1]; + collpair[index].ap3 = tri_a->tri[2]; + + collpair[index].bp1 = tri_b->tri[0]; + collpair[index].bp2 = tri_b->tri[1]; + collpair[index].bp3 = tri_b->tri[2]; + + copy_v3_v3(collpair[index].pa, pa); + copy_v3_v3(collpair[index].pb, pb); + copy_v3_v3(collpair[index].vector, vect); + + normalize_v3_v3(collpair[index].normal, collpair[index].vector); + + collpair[index].distance = distance; + collpair[index].flag = 0; + + data->collided = true; + } + else { + collpair[index].flag = COLLISION_INACTIVE; + } } -static void add_collision_object(ListBase *relations, Object *ob, int level, unsigned int modifier_type) +static void add_collision_object(ListBase *relations, + Object *ob, + int level, + unsigned int modifier_type) { - CollisionModifierData *cmd= NULL; - - /* only get objects with collision modifier */ - if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || (modifier_type != eModifierType_Collision)) - cmd= (CollisionModifierData *)modifiers_findByType(ob, modifier_type); - - if (cmd) { - CollisionRelation *relation = MEM_callocN(sizeof(CollisionRelation), "CollisionRelation"); - relation->ob = ob; - BLI_addtail(relations, relation); - } - - /* objects in dupli groups, one level only for now */ - /* TODO: this doesn't really work, we are not taking into account the - * dupli transforms and can get objects in the list multiple times. */ - if (ob->instance_collection && level == 0) { - Collection *collection= ob->instance_collection; - - /* add objects */ - FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN(collection, object) - { - add_collision_object(relations, object, level+1, modifier_type); - } - FOREACH_COLLECTION_OBJECT_RECURSIVE_END; - } + CollisionModifierData *cmd = NULL; + + /* only get objects with collision modifier */ + if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || + (modifier_type != eModifierType_Collision)) + cmd = (CollisionModifierData *)modifiers_findByType(ob, modifier_type); + + if (cmd) { + CollisionRelation *relation = MEM_callocN(sizeof(CollisionRelation), "CollisionRelation"); + relation->ob = ob; + BLI_addtail(relations, relation); + } + + /* objects in dupli groups, one level only for now */ + /* TODO: this doesn't really work, we are not taking into account the + * dupli transforms and can get objects in the list multiple times. */ + if (ob->instance_collection && level == 0) { + Collection *collection = ob->instance_collection; + + /* add objects */ + FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN (collection, object) { + add_collision_object(relations, object, level + 1, modifier_type); + } + FOREACH_COLLECTION_OBJECT_RECURSIVE_END; + } } /* Create list of collision relations in the collection or entire scene. * This is used by the depsgraph to build relations, as well as faster * lookup of colliders during evaluation. */ -ListBase *BKE_collision_relations_create(Depsgraph *depsgraph, Collection *collection, unsigned int modifier_type) +ListBase *BKE_collision_relations_create(Depsgraph *depsgraph, + Collection *collection, + unsigned int modifier_type) { - ViewLayer *view_layer = DEG_get_input_view_layer(depsgraph); - Base *base = BKE_collection_or_layer_objects(view_layer, collection); - const bool for_render = (DEG_get_mode(depsgraph) == DAG_EVAL_RENDER); - const int base_flag = (for_render) ? BASE_ENABLED_RENDER : BASE_ENABLED_VIEWPORT; + ViewLayer *view_layer = DEG_get_input_view_layer(depsgraph); + Base *base = BKE_collection_or_layer_objects(view_layer, collection); + const bool for_render = (DEG_get_mode(depsgraph) == DAG_EVAL_RENDER); + const int base_flag = (for_render) ? BASE_ENABLED_RENDER : BASE_ENABLED_VIEWPORT; - ListBase *relations = MEM_callocN(sizeof(ListBase), "CollisionRelation list"); + ListBase *relations = MEM_callocN(sizeof(ListBase), "CollisionRelation list"); - for (; base; base = base->next) { - if (base->flag & base_flag) { - add_collision_object(relations, base->object, 0, modifier_type); - } - } + for (; base; base = base->next) { + if (base->flag & base_flag) { + add_collision_object(relations, base->object, 0, modifier_type); + } + } - return relations; + return relations; } void BKE_collision_relations_free(ListBase *relations) { - if (relations) { - BLI_freelistN(relations); - MEM_freeN(relations); - } + if (relations) { + BLI_freelistN(relations); + MEM_freeN(relations); + } } /* Create effective list of colliders from relations built beforehand. * Self will be excluded. */ -Object **BKE_collision_objects_create(Depsgraph *depsgraph, Object *self, Collection *collection, unsigned int *numcollobj, unsigned int modifier_type) +Object **BKE_collision_objects_create(Depsgraph *depsgraph, + Object *self, + Collection *collection, + unsigned int *numcollobj, + unsigned int modifier_type) { - ListBase *relations = DEG_get_collision_relations(depsgraph, collection, modifier_type); - - if (!relations) { - *numcollobj = 0; - return NULL; - } - - int maxnum = BLI_listbase_count(relations); - int num = 0; - Object **objects = MEM_callocN(sizeof(Object*) * maxnum, __func__); - - for (CollisionRelation *relation = relations->first; relation; relation = relation->next) { - /* Get evaluated object. */ - Object *ob = (Object*)DEG_get_evaluated_id(depsgraph, &relation->ob->id); - - if (ob != self) { - objects[num] = ob; - num++; - } - } - - if (num == 0) { - MEM_freeN(objects); - objects = NULL; - } - - *numcollobj = num; - return objects; + ListBase *relations = DEG_get_collision_relations(depsgraph, collection, modifier_type); + + if (!relations) { + *numcollobj = 0; + return NULL; + } + + int maxnum = BLI_listbase_count(relations); + int num = 0; + Object **objects = MEM_callocN(sizeof(Object *) * maxnum, __func__); + + for (CollisionRelation *relation = relations->first; relation; relation = relation->next) { + /* Get evaluated object. */ + Object *ob = (Object *)DEG_get_evaluated_id(depsgraph, &relation->ob->id); + + if (ob != self) { + objects[num] = ob; + num++; + } + } + + if (num == 0) { + MEM_freeN(objects); + objects = NULL; + } + + *numcollobj = num; + return objects; } void BKE_collision_objects_free(Object **objects) { - if (objects) { - MEM_freeN(objects); - } + if (objects) { + MEM_freeN(objects); + } } /* Create effective list of colliders from relations built beforehand. * Self will be excluded. */ ListBase *BKE_collider_cache_create(Depsgraph *depsgraph, Object *self, Collection *collection) { - ListBase *relations = DEG_get_collision_relations(depsgraph, collection, eModifierType_Collision); - ListBase *cache = NULL; - - if (!relations) { - return NULL; - } - - for (CollisionRelation *relation = relations->first; relation; relation = relation->next) { - /* Get evaluated object. */ - Object *ob = (Object*)DEG_get_evaluated_id(depsgraph, &relation->ob->id); - - if (ob == self) { - continue; - } - - CollisionModifierData *cmd = (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision); - if (cmd && cmd->bvhtree) { - if (cache == NULL) { - cache = MEM_callocN(sizeof(ListBase), "ColliderCache array"); - } - - ColliderCache *col = MEM_callocN(sizeof(ColliderCache), "ColliderCache"); - col->ob = ob; - col->collmd = cmd; - /* make sure collider is properly set up */ - collision_move_object(cmd, 1.0, 0.0); - BLI_addtail(cache, col); - } - } - - return cache; + ListBase *relations = DEG_get_collision_relations( + depsgraph, collection, eModifierType_Collision); + ListBase *cache = NULL; + + if (!relations) { + return NULL; + } + + for (CollisionRelation *relation = relations->first; relation; relation = relation->next) { + /* Get evaluated object. */ + Object *ob = (Object *)DEG_get_evaluated_id(depsgraph, &relation->ob->id); + + if (ob == self) { + continue; + } + + CollisionModifierData *cmd = (CollisionModifierData *)modifiers_findByType( + ob, eModifierType_Collision); + if (cmd && cmd->bvhtree) { + if (cache == NULL) { + cache = MEM_callocN(sizeof(ListBase), "ColliderCache array"); + } + + ColliderCache *col = MEM_callocN(sizeof(ColliderCache), "ColliderCache"); + col->ob = ob; + col->collmd = cmd; + /* make sure collider is properly set up */ + collision_move_object(cmd, 1.0, 0.0); + BLI_addtail(cache, col); + } + } + + return cache; } void BKE_collider_cache_free(ListBase **colliders) { - if (*colliders) { - BLI_freelistN(*colliders); - MEM_freeN(*colliders); - *colliders = NULL; - } + if (*colliders) { + BLI_freelistN(*colliders); + MEM_freeN(*colliders); + *colliders = NULL; + } } -static bool cloth_bvh_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd, - CollPair **collisions, int numresult, - BVHTreeOverlap *overlap, bool culling, bool use_normal) +static bool cloth_bvh_objcollisions_nearcheck(ClothModifierData *clmd, + CollisionModifierData *collmd, + CollPair **collisions, + int numresult, + BVHTreeOverlap *overlap, + bool culling, + bool use_normal) { - *collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * numresult, "collision array"); - - ColDetectData data = { - .clmd = clmd, - .collmd = collmd, - .overlap = overlap, - .collisions = *collisions, - .culling = culling, - .use_normal = use_normal, - .collided = false, - }; - - ParallelRangeSettings settings; - BLI_parallel_range_settings_defaults(&settings); - settings.use_threading = true; - BLI_task_parallel_range(0, numresult, &data, cloth_collision, &settings); - - return data.collided; + *collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * numresult, "collision array"); + + ColDetectData data = { + .clmd = clmd, + .collmd = collmd, + .overlap = overlap, + .collisions = *collisions, + .culling = culling, + .use_normal = use_normal, + .collided = false, + }; + + ParallelRangeSettings settings; + BLI_parallel_range_settings_defaults(&settings); + settings.use_threading = true; + BLI_task_parallel_range(0, numresult, &data, cloth_collision, &settings); + + return data.collided; } -static bool cloth_bvh_selfcollisions_nearcheck(ClothModifierData * clmd, CollPair *collisions, - int numresult, BVHTreeOverlap *overlap) +static bool cloth_bvh_selfcollisions_nearcheck(ClothModifierData *clmd, + CollPair *collisions, + int numresult, + BVHTreeOverlap *overlap) { - SelfColDetectData data = { - .clmd = clmd, - .overlap = overlap, - .collisions = collisions, - .collided = false, - }; - - ParallelRangeSettings settings; - BLI_parallel_range_settings_defaults(&settings); - settings.use_threading = true; - BLI_task_parallel_range(0, numresult, &data, cloth_selfcollision, &settings); - - return data.collided; + SelfColDetectData data = { + .clmd = clmd, + .overlap = overlap, + .collisions = collisions, + .collided = false, + }; + + ParallelRangeSettings settings; + BLI_parallel_range_settings_defaults(&settings); + settings.use_threading = true; + BLI_task_parallel_range(0, numresult, &data, cloth_selfcollision, &settings); + + return data.collided; } -static int cloth_bvh_objcollisions_resolve(ClothModifierData * clmd, Object **collobjs, CollPair **collisions, - uint *collision_counts, const uint numcollobj, const float dt) +static int cloth_bvh_objcollisions_resolve(ClothModifierData *clmd, + Object **collobjs, + CollPair **collisions, + uint *collision_counts, + const uint numcollobj, + const float dt) { - Cloth *cloth = clmd->clothObject; - int i = 0, j = 0, mvert_num = 0; - ClothVertex *verts = NULL; - int ret = 0; - int result = 0; - - mvert_num = clmd->clothObject->mvert_num; - verts = cloth->verts; - - result = 1; - - for (j = 0; j < 2; j++) { - result = 0; - - for (i = 0; i < numcollobj; i++) { - Object *collob= collobjs[i]; - CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision); - - if ( collmd->bvhtree ) { - result += cloth_collision_response_static(clmd, collmd, collob, collisions[i], collision_counts[i], 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) { - add_v3_v3(verts[i].tv, verts[i].impulse); - add_v3_v3(verts[i].dcvel, verts[i].impulse); - zero_v3(verts[i].impulse); - verts[i].impulse_count = 0; - - ret++; - } - } - } - else { - break; - } - } - return ret; + Cloth *cloth = clmd->clothObject; + int i = 0, j = 0, mvert_num = 0; + ClothVertex *verts = NULL; + int ret = 0; + int result = 0; + + mvert_num = clmd->clothObject->mvert_num; + verts = cloth->verts; + + result = 1; + + for (j = 0; j < 2; j++) { + result = 0; + + for (i = 0; i < numcollobj; i++) { + Object *collob = collobjs[i]; + CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType( + collob, eModifierType_Collision); + + if (collmd->bvhtree) { + result += cloth_collision_response_static( + clmd, collmd, collob, collisions[i], collision_counts[i], 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) { + add_v3_v3(verts[i].tv, verts[i].impulse); + add_v3_v3(verts[i].dcvel, verts[i].impulse); + zero_v3(verts[i].impulse); + verts[i].impulse_count = 0; + + ret++; + } + } + } + else { + break; + } + } + return ret; } -static int cloth_bvh_selfcollisions_resolve(ClothModifierData * clmd, CollPair *collisions, int collision_count, const float dt) +static int cloth_bvh_selfcollisions_resolve(ClothModifierData *clmd, + CollPair *collisions, + int collision_count, + const float dt) { - Cloth *cloth = clmd->clothObject; - int i = 0, j = 0, mvert_num = 0; - ClothVertex *verts = NULL; - int ret = 0; - int result = 0; - - mvert_num = clmd->clothObject->mvert_num; - verts = cloth->verts; - - for (j = 0; j < 2; j++) { - result = 0; - - result += cloth_selfcollision_response_static(clmd, collisions, collision_count, dt); - - /* Apply impulses in parallel. */ - if (result) { - for (i = 0; i < mvert_num; i++) { - if (verts[i].impulse_count) { - // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count ); - add_v3_v3(verts[i].tv, verts[i].impulse); - add_v3_v3(verts[i].dcvel, verts[i].impulse); - zero_v3(verts[i].impulse); - verts[i].impulse_count = 0; - - ret++; - } - } - } - - if (!result) { - break; - } - } - return ret; + Cloth *cloth = clmd->clothObject; + int i = 0, j = 0, mvert_num = 0; + ClothVertex *verts = NULL; + int ret = 0; + int result = 0; + + mvert_num = clmd->clothObject->mvert_num; + verts = cloth->verts; + + for (j = 0; j < 2; j++) { + result = 0; + + result += cloth_selfcollision_response_static(clmd, collisions, collision_count, dt); + + /* Apply impulses in parallel. */ + if (result) { + for (i = 0; i < mvert_num; i++) { + if (verts[i].impulse_count) { + // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count ); + add_v3_v3(verts[i].tv, verts[i].impulse); + add_v3_v3(verts[i].dcvel, verts[i].impulse); + zero_v3(verts[i].impulse); + verts[i].impulse_count = 0; + + ret++; + } + } + } + + if (!result) { + break; + } + } + return ret; } -int cloth_bvh_collision(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt) +int cloth_bvh_collision( + Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt) { - Cloth *cloth = clmd->clothObject; - BVHTree *cloth_bvh = cloth->bvhtree; - uint i = 0, mvert_num = 0; - int rounds = 0; - ClothVertex *verts = NULL; - int ret = 0, ret2 = 0; - Object **collobjs = NULL; - unsigned int numcollobj = 0; - uint *coll_counts_obj = NULL; - BVHTreeOverlap **overlap_obj = NULL; - uint coll_count_self = 0; - BVHTreeOverlap *overlap_self = NULL; - - if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL) - return 0; - - verts = cloth->verts; - mvert_num = cloth->mvert_num; - - if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) { - bvhtree_update_from_cloth(clmd, false, false); - - collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision); - - if (collobjs) { - coll_counts_obj = MEM_callocN(sizeof(uint) * numcollobj, "CollCounts"); - overlap_obj = MEM_callocN(sizeof(*overlap_obj) * numcollobj, "BVHOverlap"); - - 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); - - overlap_obj[i] = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &coll_counts_obj[i], NULL, NULL); - } - } - } - - if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) { - bvhtree_update_from_cloth(clmd, false, true); - - overlap_self = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &coll_count_self, NULL, NULL); - } - - do { - ret2 = 0; - - /* Object collisions. */ - if ((clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) && collobjs) { - CollPair **collisions; - bool collided = false; - - collisions = MEM_callocN(sizeof(CollPair *) * numcollobj, "CollPair"); - - for (i = 0; i < numcollobj; i++) { - Object *collob = collobjs[i]; - CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision); - - if (!collmd->bvhtree) { - continue; - } - - if (coll_counts_obj[i] && overlap_obj[i]) { - collided = cloth_bvh_objcollisions_nearcheck(clmd, collmd, &collisions[i], coll_counts_obj[i], overlap_obj[i], - (collob->pd->flag & PFIELD_CLOTH_USE_CULLING), - (collob->pd->flag & PFIELD_CLOTH_USE_NORMAL)) || collided; - } - } - - if (collided) { - ret += cloth_bvh_objcollisions_resolve(clmd, collobjs, collisions, coll_counts_obj, numcollobj, dt); - ret2 += ret; - } - - for (i = 0; i < numcollobj; i++) { - MEM_SAFE_FREE(collisions[i]); - } - - MEM_freeN(collisions); - } - - /* Self collisions. */ - if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) { - CollPair *collisions = NULL; - - verts = cloth->verts; - mvert_num = cloth->mvert_num; - - if (cloth->bvhselftree) { - if (coll_count_self && overlap_self) { - collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * coll_count_self, "collision array"); - - if (cloth_bvh_selfcollisions_nearcheck(clmd, collisions, coll_count_self, overlap_self)) { - ret += cloth_bvh_selfcollisions_resolve(clmd, collisions, coll_count_self, dt); - ret2 += ret; - } - } - - } - - MEM_SAFE_FREE(collisions); - } - - /* Apply all collision resolution. */ - if (ret2) { - for (i = 0; i < mvert_num; i++) { - if (clmd->sim_parms->vgroup_mass > 0) { - if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) { - continue; - } - } - - add_v3_v3v3(verts[i].tx, verts[i].txold, verts[i].tv); - } - } - - rounds++; - } - while (ret2 && (clmd->coll_parms->loop_count > rounds)); - - if (overlap_obj) { - for (i = 0; i < numcollobj; i++) { - MEM_SAFE_FREE(overlap_obj[i]); - } - - MEM_freeN(overlap_obj); - } - - MEM_SAFE_FREE(coll_counts_obj); - - MEM_SAFE_FREE(overlap_self); - - BKE_collision_objects_free(collobjs); - - return MIN2(ret, 1); + Cloth *cloth = clmd->clothObject; + BVHTree *cloth_bvh = cloth->bvhtree; + uint i = 0, mvert_num = 0; + int rounds = 0; + ClothVertex *verts = NULL; + int ret = 0, ret2 = 0; + Object **collobjs = NULL; + unsigned int numcollobj = 0; + uint *coll_counts_obj = NULL; + BVHTreeOverlap **overlap_obj = NULL; + uint coll_count_self = 0; + BVHTreeOverlap *overlap_self = NULL; + + if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh == NULL) + return 0; + + verts = cloth->verts; + mvert_num = cloth->mvert_num; + + if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) { + bvhtree_update_from_cloth(clmd, false, false); + + collobjs = BKE_collision_objects_create( + depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision); + + if (collobjs) { + coll_counts_obj = MEM_callocN(sizeof(uint) * numcollobj, "CollCounts"); + overlap_obj = MEM_callocN(sizeof(*overlap_obj) * numcollobj, "BVHOverlap"); + + 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); + + overlap_obj[i] = BLI_bvhtree_overlap( + cloth_bvh, collmd->bvhtree, &coll_counts_obj[i], NULL, NULL); + } + } + } + + if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) { + bvhtree_update_from_cloth(clmd, false, true); + + overlap_self = BLI_bvhtree_overlap( + cloth->bvhselftree, cloth->bvhselftree, &coll_count_self, NULL, NULL); + } + + do { + ret2 = 0; + + /* Object collisions. */ + if ((clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) && collobjs) { + CollPair **collisions; + bool collided = false; + + collisions = MEM_callocN(sizeof(CollPair *) * numcollobj, "CollPair"); + + for (i = 0; i < numcollobj; i++) { + Object *collob = collobjs[i]; + CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType( + collob, eModifierType_Collision); + + if (!collmd->bvhtree) { + continue; + } + + if (coll_counts_obj[i] && overlap_obj[i]) { + collided = cloth_bvh_objcollisions_nearcheck( + clmd, + collmd, + &collisions[i], + coll_counts_obj[i], + overlap_obj[i], + (collob->pd->flag & PFIELD_CLOTH_USE_CULLING), + (collob->pd->flag & PFIELD_CLOTH_USE_NORMAL)) || + collided; + } + } + + if (collided) { + ret += cloth_bvh_objcollisions_resolve( + clmd, collobjs, collisions, coll_counts_obj, numcollobj, dt); + ret2 += ret; + } + + for (i = 0; i < numcollobj; i++) { + MEM_SAFE_FREE(collisions[i]); + } + + MEM_freeN(collisions); + } + + /* Self collisions. */ + if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) { + CollPair *collisions = NULL; + + verts = cloth->verts; + mvert_num = cloth->mvert_num; + + if (cloth->bvhselftree) { + if (coll_count_self && overlap_self) { + collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * coll_count_self, + "collision array"); + + if (cloth_bvh_selfcollisions_nearcheck( + clmd, collisions, coll_count_self, overlap_self)) { + ret += cloth_bvh_selfcollisions_resolve(clmd, collisions, coll_count_self, dt); + ret2 += ret; + } + } + } + + MEM_SAFE_FREE(collisions); + } + + /* Apply all collision resolution. */ + if (ret2) { + for (i = 0; i < mvert_num; i++) { + if (clmd->sim_parms->vgroup_mass > 0) { + if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) { + continue; + } + } + + add_v3_v3v3(verts[i].tx, verts[i].txold, verts[i].tv); + } + } + + rounds++; + } while (ret2 && (clmd->coll_parms->loop_count > rounds)); + + if (overlap_obj) { + for (i = 0; i < numcollobj; i++) { + MEM_SAFE_FREE(overlap_obj[i]); + } + + MEM_freeN(overlap_obj); + } + + MEM_SAFE_FREE(coll_counts_obj); + + MEM_SAFE_FREE(overlap_self); + + BKE_collision_objects_free(collobjs); + + return 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]); + 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) +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); + 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); } -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[3]) +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[3]) { - float edge1[3], edge2[3], p2face[3], p1p2[3], v0p2[3]; - float nor_v0p2, nor_p1p2; + 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); + sub_v3_v3v3(edge1, v1, v0); + sub_v3_v3v3(edge2, v2, v0); + cross_v3_v3v3(r_nor, edge1, edge2); + normalize_v3(r_nor); - sub_v3_v3v3(v0p2, p2, v0); - nor_v0p2 = dot_v3v3(v0p2, r_nor); - madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2); - interp_weights_tri_v3(r_w, v0, v1, v2, p2face); + sub_v3_v3v3(v0p2, p2, v0); + nor_v0p2 = dot_v3v3(v0p2, r_nor); + madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2); + interp_weights_tri_v3(r_w, v0, v1, v2, p2face); - sub_v3_v3v3(p1p2, p2, p1); - nor_p1p2 = dot_v3v3(p1p2, r_nor); - *r_lambda = (nor_p1p2 != 0.0f ? nor_v0p2 / nor_p1p2 : 0.0f); + sub_v3_v3v3(p1p2, p2, p1); + 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; + return r_w[1] >= 0.0f && r_w[2] >= 0.0f && r_w[1] + r_w[2] <= 1.0f; } -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) +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[3]; - - 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; + 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[3]; + + 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)) +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; + 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) +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; + int i; - /* can return 2 collisions in total */ - *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array" ); - *collisions_index = *collisions; + /* 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); - } + for (i = 0; i < numresult; i++) { + *collisions_index = cloth_point_collision( + (ModifierData *)clmd, (ModifierData *)collmd, overlap + i, epsilon, *collisions_index, dt); + } } -void cloth_find_point_contacts(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt, - ColliderContacts **r_collider_contacts, int *r_totcolliders) +void cloth_find_point_contacts(Depsgraph *depsgraph, + 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 - //////////////////////////////////////////////////////////// - - /* Check we do have collision objects to test against, before doing anything else. */ - collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision); - if (!collobjs) { - *r_collider_contacts = NULL; - *r_totcolliders = 0; - return; - } - - // create temporary cloth points bvh - cloth_bvh = BLI_bvhtree_new(mvert_num, clmd->coll_parms->epsilon, 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); - - /* 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_get_epsilon(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); - } - - BKE_collision_objects_free(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->vgroup_mass > 0) { - if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) { - continue; - } - } - - add_v3_v3v3(verts[i].tx, verts[i].txold, verts[i].tv); - } - //////////////////////////////////////////////////////////// - - *r_collider_contacts = collider_contacts; - *r_totcolliders = numcollobj; + 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 + //////////////////////////////////////////////////////////// + + /* Check we do have collision objects to test against, before doing anything else. */ + collobjs = BKE_collision_objects_create( + depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision); + if (!collobjs) { + *r_collider_contacts = NULL; + *r_totcolliders = 0; + return; + } + + // create temporary cloth points bvh + cloth_bvh = BLI_bvhtree_new(mvert_num, clmd->coll_parms->epsilon, 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); + + /* 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_get_epsilon(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); + } + + BKE_collision_objects_free(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->vgroup_mass > 0) { + if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) { + continue; + } + } + + add_v3_v3v3(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); - } + 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); + } } |