Campbell Barton
de13d0a80c
While \file doesn't need an argument, it can't have another doxy command after it.
1588 lines
45 KiB
C
1588 lines
45 KiB
C
/*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation; either version 2
|
|
* of the License, or (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software Foundation,
|
|
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
*
|
|
* The Original Code is Copyright (C) Blender Foundation
|
|
* All rights reserved.
|
|
*/
|
|
|
|
/** \file
|
|
* \ingroup bke
|
|
*/
|
|
|
|
|
|
#include "MEM_guardedalloc.h"
|
|
|
|
#include "DNA_cloth_types.h"
|
|
#include "DNA_collection_types.h"
|
|
#include "DNA_effect_types.h"
|
|
#include "DNA_object_types.h"
|
|
#include "DNA_object_force_types.h"
|
|
#include "DNA_scene_types.h"
|
|
#include "DNA_meshdata_types.h"
|
|
|
|
#include "BLI_utildefines.h"
|
|
#include "BLI_blenlib.h"
|
|
#include "BLI_math.h"
|
|
#include "BLI_task.h"
|
|
#include "BLI_threads.h"
|
|
|
|
#include "BKE_cloth.h"
|
|
#include "BKE_collection.h"
|
|
#include "BKE_effect.h"
|
|
#include "BKE_layer.h"
|
|
#include "BKE_modifier.h"
|
|
#include "BKE_scene.h"
|
|
|
|
#include "BLI_kdopbvh.h"
|
|
#include "BKE_collision.h"
|
|
|
|
#include "DEG_depsgraph.h"
|
|
#include "DEG_depsgraph_physics.h"
|
|
#include "DEG_depsgraph_query.h"
|
|
|
|
#ifdef WITH_ELTOPO
|
|
#include "eltopo-capi.h"
|
|
#endif
|
|
|
|
|
|
typedef struct ColDetectData {
|
|
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;
|
|
} SelfColDetectData;
|
|
|
|
/***********************************
|
|
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);
|
|
}
|
|
|
|
BVHTree *bvhtree_build_from_mvert(
|
|
const MVert *mvert,
|
|
const struct MVertTri *tri, int tri_num,
|
|
float epsilon)
|
|
{
|
|
BVHTree *tree;
|
|
const MVertTri *vt;
|
|
int i;
|
|
|
|
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];
|
|
|
|
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);
|
|
}
|
|
|
|
/* balance tree */
|
|
BLI_bvhtree_balance(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)
|
|
{
|
|
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);
|
|
}
|
|
|
|
/***********************************
|
|
Collision modifier code end
|
|
***********************************/
|
|
|
|
BLI_INLINE int next_ind(int i)
|
|
{
|
|
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])
|
|
{
|
|
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 )
|
|
{
|
|
/* 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;
|
|
|
|
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 );
|
|
|
|
d = ( a * c - b * b );
|
|
|
|
if ( ABS ( d ) < (double)ALMOST_ZERO ) {
|
|
*w1 = *w2 = *w3 = 1.0 / 3.0;
|
|
return;
|
|
}
|
|
|
|
w1[0] = ( float ) ( ( e * c - b * f ) / d );
|
|
|
|
if ( w1[0] < 0 )
|
|
w1[0] = 0;
|
|
|
|
w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
|
|
|
|
if ( w2[0] < 0 )
|
|
w2[0] = 0;
|
|
|
|
w3[0] = 1.0f - w1[0] - w2[0];
|
|
|
|
#undef INPR
|
|
}
|
|
|
|
#ifdef __GNUC__
|
|
# pragma GCC diagnostic push
|
|
# 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 )
|
|
{
|
|
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)
|
|
{
|
|
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++;
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
|
|
/* 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;
|
|
|
|
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;
|
|
}
|
|
|
|
#ifdef __GNUC__
|
|
# pragma GCC diagnostic pop
|
|
#endif
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
/* 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)
|
|
{
|
|
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");
|
|
|
|
for (; base; base = base->next) {
|
|
if (base->flag & base_flag) {
|
|
add_collision_object(relations, base->object, 0, modifier_type);
|
|
}
|
|
}
|
|
|
|
return relations;
|
|
}
|
|
|
|
void BKE_collision_relations_free(ListBase *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)
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
|
|
void BKE_collider_cache_free(ListBase **colliders)
|
|
{
|
|
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)
|
|
{
|
|
*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)
|
|
{
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
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]);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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;
|
|
|
|
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(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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
//Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
|
|
static CollPair *cloth_point_collision(
|
|
ModifierData *md1, ModifierData *md2,
|
|
BVHTreeOverlap *overlap, float epsilon, CollPair *collpair, float UNUSED(dt))
|
|
{
|
|
ClothModifierData *clmd = (ClothModifierData *)md1;
|
|
CollisionModifierData *collmd = (CollisionModifierData *) md2;
|
|
/* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
|
|
ClothVertex *vert = NULL;
|
|
const MVertTri *vt;
|
|
const MVert *mverts = collmd->current_x;
|
|
|
|
vert = &clmd->clothObject->verts[overlap->indexA];
|
|
vt = &collmd->tri[overlap->indexB];
|
|
|
|
collpair = cloth_point_collpair(
|
|
vert->tx, vert->x, mverts,
|
|
vt->tri[0], vt->tri[1], vt->tri[2],
|
|
overlap->indexA, overlap->indexB,
|
|
epsilon, collpair);
|
|
|
|
return collpair;
|
|
}
|
|
|
|
static void cloth_points_objcollisions_nearcheck(
|
|
ClothModifierData *clmd, CollisionModifierData *collmd,
|
|
CollPair **collisions, CollPair **collisions_index,
|
|
int numresult, BVHTreeOverlap *overlap, float epsilon, double dt)
|
|
{
|
|
int i;
|
|
|
|
/* can return 2 collisions in total */
|
|
*collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array" );
|
|
*collisions_index = *collisions;
|
|
|
|
for ( i = 0; i < numresult; i++ ) {
|
|
*collisions_index = cloth_point_collision((ModifierData *)clmd, (ModifierData *)collmd,
|
|
overlap+i, epsilon, *collisions_index, dt);
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|