diff options
Diffstat (limited to 'source/blender/physics/intern')
| -rw-r--r-- | source/blender/physics/intern/BPH_mass_spring.cpp | 346 | ||||
| -rw-r--r-- | source/blender/physics/intern/ConstrainedConjugateGradient.h | 34 | ||||
| -rw-r--r-- | source/blender/physics/intern/eigen_utils.h | 42 | ||||
| -rw-r--r-- | source/blender/physics/intern/hair_volume.cpp | 278 | ||||
| -rw-r--r-- | source/blender/physics/intern/implicit.h | 2 | ||||
| -rw-r--r-- | source/blender/physics/intern/implicit_blender.c | 436 | ||||
| -rw-r--r-- | source/blender/physics/intern/implicit_eigen.cpp | 326 |
7 files changed, 732 insertions, 732 deletions
diff --git a/source/blender/physics/intern/BPH_mass_spring.cpp b/source/blender/physics/intern/BPH_mass_spring.cpp index 5b5639495da..750073dae1f 100644 --- a/source/blender/physics/intern/BPH_mass_spring.cpp +++ b/source/blender/physics/intern/BPH_mass_spring.cpp @@ -60,7 +60,7 @@ static int cloth_count_nondiag_blocks(Cloth *cloth) { LinkNode *link; int nondiag = 0; - + for (link = cloth->springs; link; link = link->next) { ClothSpring *spring = (ClothSpring *)link->link; switch (spring->type) { @@ -68,14 +68,14 @@ static int cloth_count_nondiag_blocks(Cloth *cloth) /* angular bending combines 3 vertices */ nondiag += 3; break; - + default: /* all other springs depend on 2 vertices only */ nondiag += 1; break; } } - + return nondiag; } @@ -86,25 +86,25 @@ int BPH_cloth_solver_init(Object *UNUSED(ob), ClothModifierData *clmd) const float ZERO[3] = {0.0f, 0.0f, 0.0f}; Implicit_Data *id; unsigned int i, nondiag; - + nondiag = cloth_count_nondiag_blocks(cloth); cloth->implicit = id = BPH_mass_spring_solver_create(cloth->mvert_num, nondiag); - + for (i = 0; i < cloth->mvert_num; i++) { BPH_mass_spring_set_vertex_mass(id, i, verts[i].mass); } - + for (i = 0; i < cloth->mvert_num; i++) { BPH_mass_spring_set_motion_state(id, i, verts[i].x, ZERO); } - + return 1; } void BPH_cloth_solver_free(ClothModifierData *clmd) { Cloth *cloth = clmd->clothObject; - + if (cloth->implicit) { BPH_mass_spring_solver_free(cloth->implicit); cloth->implicit = NULL; @@ -118,7 +118,7 @@ void BKE_cloth_solver_set_positions(ClothModifierData *clmd) unsigned int mvert_num = cloth->mvert_num, i; ClothHairData *cloth_hairdata = clmd->hairdata; Implicit_Data *id = cloth->implicit; - + for (i = 0; i < mvert_num; i++) { if (cloth_hairdata) { ClothHairData *root = &cloth_hairdata[i]; @@ -126,7 +126,7 @@ void BKE_cloth_solver_set_positions(ClothModifierData *clmd) } else BPH_mass_spring_set_rest_transform(id, i, I3); - + BPH_mass_spring_set_motion_state(id, i, verts[i].x, verts[i].v); } } @@ -136,22 +136,22 @@ static bool collision_response(ClothModifierData *clmd, CollisionModifierData *c Cloth *cloth = clmd->clothObject; int index = collpair->ap1; bool result = false; - + float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3]; float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree); float margin_distance = (float)collpair->distance - epsilon2; float mag_v_rel; - + zero_v3(r_impulse); - + if (margin_distance > 0.0f) return false; /* XXX tested before already? */ - + /* only handle static collisions here */ if ( collpair->flag & COLLISION_IN_FUTURE ) return false; - + /* velocity */ copy_v3_v3(v1, cloth->verts[index].v); collision_get_collider_velocity(v2_old, v2_new, collmd, collpair); @@ -160,32 +160,32 @@ static bool collision_response(ClothModifierData *clmd, CollisionModifierData *c sub_v3_v3v3(v_rel_new, v1, v2_new); /* normal component of the relative velocity */ mag_v_rel = dot_v3v3(v_rel_old, collpair->normal); - + /* only valid when moving toward the collider */ if (mag_v_rel < -ALMOST_ZERO) { float v_nor_old, v_nor_new; float v_tan_old[3], v_tan_new[3]; float bounce, repulse; - + /* Collision response based on * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005) * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf */ - + v_nor_old = mag_v_rel; v_nor_new = dot_v3v3(v_rel_new, collpair->normal); - + madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old); madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new); - + bounce = -v_nor_old * restitution; - + repulse = -margin_distance / dt; /* base repulsion velocity in normal direction */ /* XXX this clamping factor is quite arbitrary ... * not sure if there is a more scientific approach, but seems to give good results */ CLAMP(repulse, 0.0f, 4.0f * bounce); - + if (margin_distance < -epsilon2) { mul_v3_v3fl(r_impulse, collpair->normal, max_ff(repulse, bounce) - v_nor_new); } @@ -193,10 +193,10 @@ static bool collision_response(ClothModifierData *clmd, CollisionModifierData *c bounce = 0.0f; mul_v3_v3fl(r_impulse, collpair->normal, repulse - v_nor_new); } - + result = true; } - + return result; } @@ -211,17 +211,17 @@ static void cloth_setup_constraints(ClothModifierData *clmd, ColliderContacts *c ClothVertex *verts = cloth->verts; int mvert_num = cloth->mvert_num; int i, j, v; - + const float ZERO[3] = {0.0f, 0.0f, 0.0f}; - + BPH_mass_spring_clear_constraints(data); - + for (v = 0; v < mvert_num; v++) { if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) { /* pinned vertex constraints */ BPH_mass_spring_add_constraint_ndof0(data, v, ZERO); /* velocity is defined externally */ } - + verts[v].impulse_count = 0; } @@ -233,21 +233,21 @@ static void cloth_setup_constraints(ClothModifierData *clmd, ColliderContacts *c float restitution = 0.0f; int v = collpair->face1; float impulse[3]; - + /* pinned verts handled separately */ if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) continue; - + /* XXX cheap way of avoiding instability from multiple collisions in the same step * this should eventually be supported ... */ if (verts[v].impulse_count > 0) continue; - + /* calculate collision response */ if (!collision_response(clmd, ct->collmd, collpair, dt, restitution, impulse)) continue; - + BPH_mass_spring_add_constraint_ndof2(data, v, collpair->normal, impulse); ++verts[v].impulse_count; } @@ -267,11 +267,11 @@ static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothMo ClothSpring *spring; ClothVertex *cv; int i, steps; - + cv = cloth->verts; for (i = 0; i < cloth->mvert_num; i++, cv++) { copy_v3_v3(cos[i], cv->tx); - + if (cv->goal == 1.0f || len_squared_v3v3(initial_cos[i], cv->tx) != 0.0f) { masses[i] = 1e+10; } @@ -279,57 +279,57 @@ static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothMo masses[i] = cv->mass; } } - + steps = 55; for (i=0; i<steps; i++) { for (node=cloth->springs; node; node=node->next) { /* ClothVertex *cv1, *cv2; */ /* UNUSED */ int v1, v2; float len, c, l, vec[3]; - + spring = (ClothSpring *)node->link; - if (spring->type != CLOTH_SPRING_TYPE_STRUCTURAL && spring->type != CLOTH_SPRING_TYPE_SHEAR) + if (spring->type != CLOTH_SPRING_TYPE_STRUCTURAL && spring->type != CLOTH_SPRING_TYPE_SHEAR) continue; - + v1 = spring->ij; v2 = spring->kl; /* cv1 = cloth->verts + v1; */ /* UNUSED */ /* cv2 = cloth->verts + v2; */ /* UNUSED */ len = len_v3v3(cos[v1], cos[v2]); - + sub_v3_v3v3(vec, cos[v1], cos[v2]); normalize_v3(vec); - + c = (len - spring->restlen); if (c == 0.0f) continue; - + l = c / ((1.0f / masses[v1]) + (1.0f / masses[v2])); - + mul_v3_fl(vec, -(1.0f / masses[v1]) * l); add_v3_v3(cos[v1], vec); - + sub_v3_v3v3(vec, cos[v2], cos[v1]); normalize_v3(vec); - + mul_v3_fl(vec, -(1.0f / masses[v2]) * l); add_v3_v3(cos[v2], vec); } } - + cv = cloth->verts; for (i = 0; i < cloth->mvert_num; i++, cv++) { float vec[3]; - + /*compute forces*/ sub_v3_v3v3(vec, cos[i], cv->tx); mul_v3_fl(vec, cv->mass*dt*20.0f); add_v3_v3(cv->tv, vec); //copy_v3_v3(cv->tx, cos[i]); } - + MEM_freeN(cos); MEM_freeN(masses); - + return 1; } @@ -338,21 +338,21 @@ BLI_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s) Cloth *cloth = clmd->clothObject; ClothSimSettings *parms = clmd->sim_parms; Implicit_Data *data = cloth->implicit; - + bool no_compress = parms->flags & CLOTH_SIMSETTINGS_FLAG_NO_SPRING_COMPRESS; - + s->flags &= ~CLOTH_SPRING_FLAG_NEEDED; - + // calculate force of structural + shear springs if ((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR) || (s->type & CLOTH_SPRING_TYPE_SEWING) ) { #ifdef CLOTH_FORCE_SPRING_STRUCTURAL float k, scaling; - + s->flags |= CLOTH_SPRING_FLAG_NEEDED; - + scaling = parms->structural + s->stiffness * fabsf(parms->max_struct - parms->structural); k = scaling / (parms->avg_spring_len + FLT_EPSILON); - + if (s->type & CLOTH_SPRING_TYPE_SEWING) { // TODO: verify, half verified (couldn't see error) // sewing springs usually have a large distance at first so clamp the force so we don't get tunnelling through colission objects @@ -366,50 +366,50 @@ BLI_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s) else if (s->type & CLOTH_SPRING_TYPE_BENDING) { /* calculate force of bending springs */ #ifdef CLOTH_FORCE_SPRING_BEND float kb, cb, scaling; - + s->flags |= CLOTH_SPRING_FLAG_NEEDED; - + scaling = parms->bending + s->stiffness * fabsf(parms->max_bend - parms->bending); kb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON)); - + // Fix for [#45084] for cloth stiffness must have cb proportional to kb cb = kb * parms->bending_damping; - + BPH_mass_spring_force_spring_bending(data, s->ij, s->kl, s->restlen, kb, cb); #endif } else if (s->type & CLOTH_SPRING_TYPE_BENDING_ANG) { #ifdef CLOTH_FORCE_SPRING_BEND float kb, cb, scaling; - + s->flags |= CLOTH_SPRING_FLAG_NEEDED; - + /* XXX WARNING: angular bending springs for hair apply stiffness factor as an overall factor, unlike cloth springs! * this is crap, but needed due to cloth/hair mixing ... * max_bend factor is not even used for hair, so ... */ scaling = s->stiffness * parms->bending; kb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON)); - + // Fix for [#45084] for cloth stiffness must have cb proportional to kb cb = kb * parms->bending_damping; - + /* XXX assuming same restlen for ij and jk segments here, this can be done correctly for hair later */ BPH_mass_spring_force_spring_bending_angular(data, s->ij, s->kl, s->mn, s->target, kb, cb); - + #if 0 { float x_kl[3], x_mn[3], v[3], d[3]; - + BPH_mass_spring_get_motion_state(data, s->kl, x_kl, v); BPH_mass_spring_get_motion_state(data, s->mn, x_mn, v); - + BKE_sim_debug_data_add_dot(clmd->debug_data, x_kl, 0.9, 0.9, 0.9, "target", 7980, s->kl); BKE_sim_debug_data_add_line(clmd->debug_data, x_kl, x_mn, 0.8, 0.8, 0.8, "target", 7981, s->kl); - + copy_v3_v3(d, s->target); BKE_sim_debug_data_add_vector(clmd->debug_data, x_kl, d, 0.8, 0.8, 0.2, "target", 7982, s->kl); - + // copy_v3_v3(d, s->target_ij); // BKE_sim_debug_data_add_vector(clmd->debug_data, x, d, 1, 0.4, 0.4, "target", 7983, s->kl); } @@ -424,7 +424,7 @@ static void hair_get_boundbox(ClothModifierData *clmd, float gmin[3], float gmax Implicit_Data *data = cloth->implicit; unsigned int mvert_num = cloth->mvert_num; int i; - + INIT_MINMAX(gmin, gmax); for (i = 0; i < mvert_num; i++) { float x[3]; @@ -444,7 +444,7 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListB const MVertTri *tri = cloth->tri; unsigned int mvert_num = cloth->mvert_num; ClothVertex *vert; - + #ifdef CLOTH_FORCE_GRAVITY /* global acceleration (gravitation) */ if (clmd->scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) { @@ -477,7 +477,7 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListB #ifdef CLOTH_FORCE_DRAG BPH_mass_spring_force_drag(data, drag); #endif - + /* handle external forces like wind */ if (effectors) { /* cache per-vertex forces to avoid redundant calculation */ @@ -485,12 +485,12 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListB for (i = 0; i < cloth->mvert_num; i++) { float x[3], v[3]; EffectedPoint epoint; - + BPH_mass_spring_get_motion_state(data, i, x, v); pd_point_from_loc(clmd->scene, x, v, i, &epoint); pdDoEffectors(effectors, NULL, clmd->sim_parms->effector_weights, &epoint, winvec[i], NULL); } - + for (i = 0; i < cloth->tri_num; i++) { const MVertTri *vt = &tri[i]; BPH_mass_spring_force_face_wind(data, vt->tri[0], vt->tri[1], vt->tri[2], winvec); @@ -501,7 +501,7 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListB #if 0 ClothHairData *hairdata = clmd->hairdata; ClothHairData *hair_ij, *hair_kl; - + for (LinkNode *link = cloth->springs; link; link = link->next) { ClothSpring *spring = (ClothSpring *)link->link; if (spring->type == CLOTH_SPRING_TYPE_STRUCTURAL) { @@ -516,7 +516,7 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListB } #else ClothHairData *hairdata = clmd->hairdata; - + vert = cloth->verts; for (i = 0; i < cloth->mvert_num; i++, vert++) { if (hairdata) { @@ -531,7 +531,7 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListB MEM_freeN(winvec); } - + // calculate spring forces for (LinkNode *link = cloth->springs; link; link = link->next) { ClothSpring *spring = (ClothSpring *)link->link; @@ -548,7 +548,7 @@ BLI_INLINE void cloth_get_grid_location(Implicit_Data *data, float cell_scale, c { BPH_mass_spring_get_position(data, index, x); BPH_mass_spring_get_new_velocity(data, index, v); - + mul_v3_fl(x, cell_scale); add_v3_v3(x, cell_offset); } @@ -582,41 +582,41 @@ static LinkNode *cloth_continuum_add_hair_segments(HairGrid *grid, const float c // ClothVertex *vert3, *vert4; float x1[3], v1[3], x2[3], v2[3], x3[3], v3[3], x4[3], v4[3]; float dir1[3], dir2[3], dir3[3]; - + spring1 = NULL; spring2 = NULL; spring3 = (ClothSpring *)spring_link->link; - + zero_v3(x1); zero_v3(v1); zero_v3(dir1); zero_v3(x2); zero_v3(v2); zero_v3(dir2); - + // vert3 = &verts[spring3->kl]; cloth_get_grid_location(data, cell_scale, cell_offset, spring3->kl, x3, v3); // vert4 = &verts[spring3->ij]; cloth_get_grid_location(data, cell_scale, cell_offset, spring3->ij, x4, v4); sub_v3_v3v3(dir3, x4, x3); normalize_v3(dir3); - + while (spring_link) { /* move on */ spring1 = spring2; spring2 = spring3; - + // vert3 = vert4; - + copy_v3_v3(x1, x2); copy_v3_v3(v1, v2); copy_v3_v3(x2, x3); copy_v3_v3(v2, v3); copy_v3_v3(x3, x4); copy_v3_v3(v3, v4); - + copy_v3_v3(dir1, dir2); copy_v3_v3(dir2, dir3); - + /* read next segment */ next_spring_link = spring_link->next; spring_link = hair_spring_next(spring_link); - + if (spring_link) { spring3 = (ClothSpring *)spring_link->link; // vert4 = &verts[spring3->ij]; @@ -630,13 +630,13 @@ static LinkNode *cloth_continuum_add_hair_segments(HairGrid *grid, const float c zero_v3(x4); zero_v3(v4); zero_v3(dir3); } - + BPH_hair_volume_add_segment(grid, x1, v1, x2, v2, x3, v3, x4, v4, spring1 ? dir1 : NULL, dir2, spring3 ? dir3 : NULL); } - + return next_spring_link; } @@ -647,17 +647,17 @@ static void cloth_continuum_fill_grid(HairGrid *grid, Cloth *cloth) int mvert_num = cloth->mvert_num; ClothVertex *vert; int i; - + for (i = 0, vert = cloth->verts; i < mvert_num; i++, vert++) { float x[3], v[3]; - + cloth_get_vertex_motion_state(data, vert, x, v); BPH_hair_volume_add_vertex(grid, x, v); } #else LinkNode *link; float cellsize, gmin[3], cell_scale, cell_offset[3]; - + /* scale and offset for transforming vertex locations into grid space * (cell size is 0..1, gmin becomes origin) */ @@ -665,7 +665,7 @@ static void cloth_continuum_fill_grid(HairGrid *grid, Cloth *cloth) cell_scale = cellsize > 0.0f ? 1.0f / cellsize : 0.0f; mul_v3_v3fl(cell_offset, gmin, cell_scale); negate_v3(cell_offset); - + link = cloth->springs; while (link) { ClothSpring *spring = (ClothSpring *)link->link; @@ -685,7 +685,7 @@ static void cloth_continuum_step(ClothModifierData *clmd, float dt) Implicit_Data *data = cloth->implicit; int mvert_num = cloth->mvert_num; ClothVertex *vert; - + const float fluid_factor = 0.95f; /* blend between PIC and FLIP methods */ float smoothfac = parms->velocity_smooth; /* XXX FIXME arbitrary factor!!! this should be based on some intuitive value instead, @@ -695,42 +695,42 @@ static void cloth_continuum_step(ClothModifierData *clmd, float dt) float density_strength = parms->density_strength; float gmin[3], gmax[3]; int i; - + /* clear grid info */ zero_v3_int(clmd->hair_grid_res); zero_v3(clmd->hair_grid_min); zero_v3(clmd->hair_grid_max); clmd->hair_grid_cellsize = 0.0f; - + hair_get_boundbox(clmd, gmin, gmax); - + /* gather velocities & density */ if (smoothfac > 0.0f || density_strength > 0.0f) { HairGrid *grid = BPH_hair_volume_create_vertex_grid(clmd->sim_parms->voxel_cell_size, gmin, gmax); - + cloth_continuum_fill_grid(grid, cloth); - + /* main hair continuum solver */ BPH_hair_volume_solve_divergence(grid, dt, density_target, density_strength); - + for (i = 0, vert = cloth->verts; i < mvert_num; i++, vert++) { float x[3], v[3], nv[3]; - + /* calculate volumetric velocity influence */ BPH_mass_spring_get_position(data, i, x); BPH_mass_spring_get_new_velocity(data, i, v); - + BPH_hair_volume_grid_velocity(grid, x, v, fluid_factor, nv); - + interp_v3_v3v3(nv, v, nv, smoothfac); - + /* apply on hair data */ BPH_mass_spring_set_new_velocity(data, i, nv); } - + /* store basic grid info in the modifier data */ BPH_hair_volume_grid_geometry(grid, &clmd->hair_grid_cellsize, clmd->hair_grid_res, clmd->hair_grid_min, clmd->hair_grid_max); - + #if 0 /* DEBUG hair velocity vector field */ { const int size = 64; @@ -738,26 +738,26 @@ static void cloth_continuum_step(ClothModifierData *clmd, float dt) float offset[3], a[3], b[3]; const int axis = 0; const float shift = 0.0f; - + copy_v3_v3(offset, clmd->hair_grid_min); zero_v3(a); zero_v3(b); - + offset[axis] = shift * clmd->hair_grid_cellsize; a[(axis+1) % 3] = clmd->hair_grid_max[(axis+1) % 3] - clmd->hair_grid_min[(axis+1) % 3]; b[(axis+2) % 3] = clmd->hair_grid_max[(axis+2) % 3] - clmd->hair_grid_min[(axis+2) % 3]; - + BKE_sim_debug_data_clear_category(clmd->debug_data, "grid velocity"); for (j = 0; j < size; ++j) { for (i = 0; i < size; ++i) { float x[3], v[3], gvel[3], gvel_smooth[3], gdensity; - + madd_v3_v3v3fl(x, offset, a, (float)i / (float)(size-1)); madd_v3_v3fl(x, b, (float)j / (float)(size-1)); zero_v3(v); - + BPH_hair_volume_grid_interpolate(grid, x, &gdensity, gvel, gvel_smooth, NULL, NULL); - + // BKE_sim_debug_data_add_circle(clmd->debug_data, x, gdensity, 0.7, 0.3, 1, "grid density", i, j, 3111); if (!is_zero_v3(gvel) || !is_zero_v3(gvel_smooth)) { float dvel[3]; @@ -770,7 +770,7 @@ static void cloth_continuum_step(ClothModifierData *clmd, float dt) float col0[3] = {0.0, 0.0, 0.0}; float col1[3] = {0.0, 1.0, 0.0}; float col[3]; - + interp_v3_v3v3(col, col0, col1, CLAMPIS(gdensity * clmd->sim_parms->density_strength, 0.0, 1.0)); // BKE_sim_debug_data_add_circle(clmd->debug_data, x, gdensity * clmd->sim_parms->density_strength, 0, 1, 0.4, "grid velocity", i, j, 3115); // BKE_sim_debug_data_add_dot(clmd->debug_data, x, col[0], col[1], col[2], "grid velocity", i, j, 3115); @@ -782,7 +782,7 @@ static void cloth_continuum_step(ClothModifierData *clmd, float dt) } } #endif - + BPH_hair_volume_free_vertex_grid(grid); } } @@ -795,7 +795,7 @@ static void cloth_calc_volume_force(ClothModifierData *clmd) Implicit_Data *data = cloth->implicit; int mvert_num = cloth->mvert_num; ClothVertex *vert; - + /* 2.0f is an experimental value that seems to give good results */ float smoothfac = 2.0f * parms->velocity_smooth; float collfac = 2.0f * parms->collider_friction; @@ -803,17 +803,17 @@ static void cloth_calc_volume_force(ClothModifierData *clmd) float minpress = parms->pressure_threshold; float gmin[3], gmax[3]; int i; - + hair_get_boundbox(clmd, gmin, gmax); - + /* gather velocities & density */ if (smoothfac > 0.0f || pressfac > 0.0f) { HairVertexGrid *vertex_grid = BPH_hair_volume_create_vertex_grid(clmd->sim_parms->voxel_res, gmin, gmax); - + vert = cloth->verts; for (i = 0; i < mvert_num; i++, vert++) { float x[3], v[3]; - + if (vert->solver_index < 0) { copy_v3_v3(x, vert->x); copy_v3_v3(v, vert->v); @@ -824,21 +824,21 @@ static void cloth_calc_volume_force(ClothModifierData *clmd) BPH_hair_volume_add_vertex(vertex_grid, x, v); } BPH_hair_volume_normalize_vertex_grid(vertex_grid); - + vert = cloth->verts; for (i = 0; i < mvert_num; i++, vert++) { float x[3], v[3], f[3], dfdx[3][3], dfdv[3][3]; - + if (vert->solver_index < 0) continue; - + /* calculate volumetric forces */ BPH_mass_spring_get_motion_state(data, vert->solver_index, x, v); BPH_hair_volume_vertex_grid_forces(vertex_grid, x, v, smoothfac, pressfac, minpress, f, dfdx, dfdv); /* apply on hair data */ BPH_mass_spring_force_extern(data, vert->solver_index, f, dfdx, dfdv); } - + BPH_hair_volume_free_vertex_grid(vertex_grid); } } @@ -854,33 +854,33 @@ static void cloth_collision_solve_extra(Object *ob, ClothModifierData *clmd, Lis ClothVertex *verts = cloth->verts; int mvert_num = cloth->mvert_num; const float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; - + bool do_extra_solve; int i; - + if (!(clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED)) return; if (!clmd->clothObject->bvhtree) return; - + // update verts to current positions for (i = 0; i < mvert_num; i++) { BPH_mass_spring_get_new_position(id, i, verts[i].tx); - + sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold); copy_v3_v3(verts[i].v, verts[i].tv); } - + #if 0 /* unused */ for (i=0, cv=cloth->verts; i<cloth->mvert_num; i++, cv++) { copy_v3_v3(initial_cos[i], cv->tx); } #endif - + // call collision function // TODO: check if "step" or "step+dt" is correct - dg do_extra_solve = cloth_bvh_objcollision(ob, clmd, step / clmd->sim_parms->timescale, dt / clmd->sim_parms->timescale); - + // copy corrected positions back to simulation for (i = 0; i < mvert_num; i++) { float curx[3]; @@ -888,41 +888,41 @@ static void cloth_collision_solve_extra(Object *ob, ClothModifierData *clmd, Lis // correct velocity again, just to be sure we had to change it due to adaptive collisions sub_v3_v3v3(verts[i].tv, verts[i].tx, curx); } - + if (do_extra_solve) { // cloth_calc_helper_forces(ob, clmd, initial_cos, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale); - + for (i = 0; i < mvert_num; i++) { - + float newv[3]; - + if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED)) continue; - + BPH_mass_spring_set_new_position(id, i, verts[i].tx); mul_v3_v3fl(newv, verts[i].tv, spf); BPH_mass_spring_set_new_velocity(id, i, newv); } } - + // X = Xnew; BPH_mass_spring_apply_result(id); - + if (do_extra_solve) { ImplicitSolverResult result; - + /* initialize forces to zero */ BPH_mass_spring_clear_forces(id); - + // calculate forces cloth_calc_force(clmd, frame, effectors, step); - + // calculate new velocity and position BPH_mass_spring_solve_velocities(id, dt, &result); // cloth_record_result(clmd, &result, clmd->sim_parms->stepsPerFrame); - + /* note: positions are advanced only once in the main solver step! */ - + BPH_mass_spring_apply_result(id); } } @@ -930,7 +930,7 @@ static void cloth_collision_solve_extra(Object *ob, ClothModifierData *clmd, Lis static void cloth_clear_result(ClothModifierData *clmd) { ClothSolverResult *sres = clmd->solver_result; - + sres->status = 0; sres->max_error = sres->min_error = sres->avg_error = 0.0f; sres->max_iterations = sres->min_iterations = 0; @@ -940,7 +940,7 @@ static void cloth_clear_result(ClothModifierData *clmd) static void cloth_record_result(ClothModifierData *clmd, ImplicitSolverResult *result, int steps) { ClothSolverResult *sres = clmd->solver_result; - + if (sres->status) { /* already initialized ? */ /* error only makes sense for successful iterations */ if (result->status == BPH_SOLVER_SUCCESS) { @@ -948,7 +948,7 @@ static void cloth_record_result(ClothModifierData *clmd, ImplicitSolverResult *r sres->max_error = max_ff(sres->max_error, result->error); sres->avg_error += result->error / (float)steps; } - + sres->min_iterations = min_ii(sres->min_iterations, result->iterations); sres->max_iterations = max_ii(sres->max_iterations, result->iterations); sres->avg_iterations += (float)result->iterations / (float)steps; @@ -959,11 +959,11 @@ static void cloth_record_result(ClothModifierData *clmd, ImplicitSolverResult *r sres->min_error = sres->max_error = result->error; sres->avg_error += result->error / (float)steps; } - + sres->min_iterations = sres->max_iterations = result->iterations; sres->avg_iterations += (float)result->iterations / (float)steps; } - + sres->status |= result->status; } @@ -974,7 +974,7 @@ int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase * * Bad design, TODO */ const bool is_hair = (clmd->hairdata != NULL); - + unsigned int i=0; float step=0.0f, tf=clmd->sim_parms->timescale; Cloth *cloth = clmd->clothObject; @@ -984,13 +984,13 @@ int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase * Implicit_Data *id = cloth->implicit; ColliderContacts *contacts = NULL; int totcolliders = 0; - + BKE_sim_debug_data_clear_category("collision"); - + if (!clmd->solver_result) clmd->solver_result = (ClothSolverResult *)MEM_callocN(sizeof(ClothSolverResult), "cloth solver result"); cloth_clear_result(clmd); - + if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */ for (i = 0; i < mvert_num; i++) { // update velocities with constrained velocities from pinned verts @@ -1004,22 +1004,22 @@ int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase * } } } - + while (step < tf) { ImplicitSolverResult result; - + /* copy velocities for collision */ for (i = 0; i < mvert_num; i++) { BPH_mass_spring_get_motion_state(id, i, NULL, verts[i].tv); copy_v3_v3(verts[i].v, verts[i].tv); } - + if (is_hair) { /* determine contact points */ if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) { cloth_find_point_contacts(ob, clmd, 0.0f, tf, &contacts, &totcolliders); } - + /* setup vertex constraints for pinned vertices and contacts */ cloth_setup_constraints(clmd, contacts, totcolliders, dt); } @@ -1027,10 +1027,10 @@ int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase * /* setup vertex constraints for pinned vertices */ cloth_setup_constraints(clmd, NULL, 0, dt); } - + /* initialize forces to zero */ BPH_mass_spring_clear_forces(id); - + // damping velocity for artistic reasons // this is a bad way to do it, should be removed imo - lukas_t if (clmd->sim_parms->vel_damping != 1.0f) { @@ -1041,26 +1041,26 @@ int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase * BPH_mass_spring_set_velocity(id, i, v); } } - + // calculate forces cloth_calc_force(clmd, frame, effectors, step); - + // calculate new velocity and position BPH_mass_spring_solve_velocities(id, dt, &result); cloth_record_result(clmd, &result, clmd->sim_parms->stepsPerFrame); - + if (is_hair) { cloth_continuum_step(clmd, dt); } - + BPH_mass_spring_solve_positions(id, dt); - + if (!is_hair) { cloth_collision_solve_extra(ob, clmd, effectors, frame, step, dt); } - + BPH_mass_spring_apply_result(id); - + /* move pinned verts to correct position */ for (i = 0; i < mvert_num; i++) { if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { @@ -1071,24 +1071,24 @@ int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase * BPH_mass_spring_set_position(id, i, x); } } - + BPH_mass_spring_get_motion_state(id, i, verts[i].txold, NULL); } - + /* free contact points */ if (contacts) { cloth_free_contacts(contacts, totcolliders); } - + step += dt; } - + /* copy results back to cloth data */ for (i = 0; i < mvert_num; i++) { BPH_mass_spring_get_motion_state(id, i, verts[i].x, verts[i].v); copy_v3_v3(verts[i].txold, verts[i].x); } - + return 1; } @@ -1097,18 +1097,18 @@ bool BPH_cloth_solver_get_texture_data(Object *UNUSED(ob), ClothModifierData *cl Cloth *cloth = clmd->clothObject; HairGrid *grid; float gmin[3], gmax[3]; - + if (!clmd->clothObject || !clmd->clothObject->implicit) return false; - + hair_get_boundbox(clmd, gmin, gmax); - + grid = BPH_hair_volume_create_vertex_grid(clmd->sim_parms->voxel_cell_size, gmin, gmax); cloth_continuum_fill_grid(grid, cloth); - + BPH_hair_volume_get_texture_data(grid, vd); - + BPH_hair_volume_free_vertex_grid(grid); - + return true; } diff --git a/source/blender/physics/intern/ConstrainedConjugateGradient.h b/source/blender/physics/intern/ConstrainedConjugateGradient.h index f9c6931fe8c..2d5fb41cc73 100644 --- a/source/blender/physics/intern/ConstrainedConjugateGradient.h +++ b/source/blender/physics/intern/ConstrainedConjugateGradient.h @@ -4,7 +4,7 @@ #include <Eigen/Core> -namespace Eigen { +namespace Eigen { namespace internal { @@ -29,16 +29,16 @@ void constrained_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& typedef typename Dest::RealScalar RealScalar; typedef typename Dest::Scalar Scalar; typedef Matrix<Scalar,Dynamic,1> VectorType; - + RealScalar tol = tol_error; int maxIters = iters; - + int n = mat.cols(); VectorType residual = filter * (rhs - mat * x); //initial residual RealScalar rhsNorm2 = (filter * rhs).squaredNorm(); - if(rhsNorm2 == 0) + if(rhsNorm2 == 0) { /* XXX TODO set constrained result here */ x.setZero(); @@ -54,7 +54,7 @@ void constrained_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& tol_error = sqrt(residualNorm2 / rhsNorm2); return; } - + VectorType p(n); p = filter * precond.solve(residual); //initial search direction @@ -68,11 +68,11 @@ void constrained_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& Scalar alpha = absNew / p.dot(tmp); // the amount we travel on dir x += alpha * p; // update solution residual -= alpha * tmp; // update residue - + residualNorm2 = residual.squaredNorm(); if(residualNorm2 < threshold) break; - + z = precond.solve(residual); // approximately solve for "A z = residual" RealScalar absOld = absNew; @@ -95,20 +95,20 @@ struct MatrixFilter m_cmat(NULL) { } - + MatrixFilter(const MatrixType &cmat) : m_cmat(&cmat) { } - + void setMatrix(const MatrixType &cmat) { m_cmat = &cmat; } - + template <typename VectorType> void apply(VectorType v) const { v = (*m_cmat) * v; } - + protected: const MatrixType *m_cmat; }; @@ -145,7 +145,7 @@ struct traits<ConstrainedConjugateGradient<_MatrixType,_UpLo,_FilterMatrixType,_ * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations * and NumTraits<Scalar>::epsilon() for the tolerance. - * + * * This class can be used as the direct solver classes. Here is a typical usage example: * \code * int n = 10000; @@ -160,7 +160,7 @@ struct traits<ConstrainedConjugateGradient<_MatrixType,_UpLo,_FilterMatrixType,_ * // update b, and solve again * x = cg.solve(b); * \endcode - * + * * By default the iterations start with x=0 as an initial guess of the solution. * One can control the start using the solveWithGuess() method. Here is a step by * step execution example starting with a random guess and printing the evolution @@ -176,7 +176,7 @@ struct traits<ConstrainedConjugateGradient<_MatrixType,_UpLo,_FilterMatrixType,_ * } while (cg.info()!=Success && i<100); * \endcode * Note that such a step by step excution is slightly slower. - * + * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ template< typename _MatrixType, int _UpLo, typename _FilterMatrixType, typename _Preconditioner> @@ -206,10 +206,10 @@ public: ConstrainedConjugateGradient() : Base() {} /** Initialize the solver with matrix \a A for further \c Ax=b solving. - * + * * This constructor is a shortcut for the default constructor followed * by a call to compute(). - * + * * \warning this class stores a reference to the matrix A as well as some * precomputed values that depend on it. Therefore, if \a A is changed * this class becomes invalid. Call compute() to update it with the new @@ -258,7 +258,7 @@ public: m_isInitialized = true; m_info = m_error <= Base::m_tolerance ? Success : NoConvergence; } - + /** \internal */ template<typename Rhs,typename Dest> void _solve(const Rhs& b, Dest& x) const diff --git a/source/blender/physics/intern/eigen_utils.h b/source/blender/physics/intern/eigen_utils.h index 8a5a9dbf5e9..4598d3ad3a7 100644 --- a/source/blender/physics/intern/eigen_utils.h +++ b/source/blender/physics/intern/eigen_utils.h @@ -56,24 +56,24 @@ typedef float Scalar; class Vector3 : public Eigen::Vector3f { public: typedef float *ctype; - + Vector3() { } - + Vector3(const ctype &v) { for (int k = 0; k < 3; ++k) coeffRef(k) = v[k]; } - + Vector3& operator = (const ctype &v) { for (int k = 0; k < 3; ++k) coeffRef(k) = v[k]; return *this; } - + operator ctype() { return data(); @@ -86,18 +86,18 @@ public: class Matrix3 : public Eigen::Matrix3f { public: typedef float (*ctype)[3]; - + Matrix3() { } - + Matrix3(const ctype &v) { for (int k = 0; k < 3; ++k) for (int l = 0; l < 3; ++l) coeffRef(l, k) = v[k][l]; } - + Matrix3& operator = (const ctype &v) { for (int k = 0; k < 3; ++k) @@ -105,7 +105,7 @@ public: coeffRef(l, k) = v[k][l]; return *this; } - + operator ctype() { return (ctype)data(); @@ -120,23 +120,23 @@ typedef Eigen::VectorXf lVector; class lVector3f : public Eigen::VectorXf { public: typedef Eigen::VectorXf base_t; - + lVector3f() { } - + template <typename T> lVector3f& operator = (T rhs) { base_t::operator=(rhs); return *this; } - + float* v3(int vertex) { return &coeffRef(3 * vertex); } - + const float* v3(int vertex) const { return &coeffRef(3 * vertex); @@ -157,18 +157,18 @@ struct lMatrix3fCtor { lMatrix3fCtor() { } - + void reset() { m_trips.clear(); } - + void reserve(int numverts) { /* reserve for diagonal entries */ m_trips.reserve(numverts * 9); } - + void add(int i, int j, const Matrix3 &m) { i *= 3; @@ -177,7 +177,7 @@ struct lMatrix3fCtor { for (int l = 0; l < 3; ++l) m_trips.push_back(Triplet(i + k, j + l, m.coeff(l, k))); } - + void sub(int i, int j, const Matrix3 &m) { i *= 3; @@ -186,13 +186,13 @@ struct lMatrix3fCtor { for (int l = 0; l < 3; ++l) m_trips.push_back(Triplet(i + k, j + l, -m.coeff(l, k))); } - + inline void construct(lMatrix &m) { m.setFromTriplets(m_trips.begin(), m_trips.end()); m_trips.clear(); } - + private: TripletList m_trips; }; @@ -206,7 +206,7 @@ BLI_INLINE void print_lvector(const lVector3f &v) for (int i = 0; i < v.rows(); ++i) { if (i > 0 && i % 3 == 0) printf("\n"); - + printf("%f,\n", v[i]); } } @@ -216,11 +216,11 @@ BLI_INLINE void print_lmatrix(const lMatrix &m) for (int j = 0; j < m.rows(); ++j) { if (j > 0 && j % 3 == 0) printf("\n"); - + for (int i = 0; i < m.cols(); ++i) { if (i > 0 && i % 3 == 0) printf(" "); - + implicit_print_matrix_elem(m.coeff(j, i)); } printf("\n"); diff --git a/source/blender/physics/intern/hair_volume.cpp b/source/blender/physics/intern/hair_volume.cpp index ad6fbed32e0..cd869046cc1 100644 --- a/source/blender/physics/intern/hair_volume.cpp +++ b/source/blender/physics/intern/hair_volume.cpp @@ -82,7 +82,7 @@ typedef struct HairGridVert { int samples; float velocity[3]; float density; - + float velocity_smooth[3]; } HairGridVert; @@ -107,20 +107,20 @@ BLI_INLINE int hair_grid_offset(const float vec[3], const int res[3], const floa BLI_INLINE int hair_grid_interp_weights(const int res[3], const float gmin[3], float scale, const float vec[3], float uvw[3]) { int i, j, k, offset; - + i = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 0); j = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 1); k = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 2); offset = i + (j + k*res[1])*res[0]; - + uvw[0] = (vec[0] - gmin[0]) * scale - (float)i; uvw[1] = (vec[1] - gmin[1]) * scale - (float)j; uvw[2] = (vec[2] - gmin[2]) * scale - (float)k; - + // BLI_assert(0.0f <= uvw[0] && uvw[0] <= 1.0001f); // BLI_assert(0.0f <= uvw[1] && uvw[1] <= 1.0001f); // BLI_assert(0.0f <= uvw[2] && uvw[2] <= 1.0001f); - + return offset; } @@ -131,12 +131,12 @@ BLI_INLINE void hair_grid_interpolate(const HairGridVert *grid, const int res[3] float uvw[3], muvw[3]; int res2 = res[1] * res[0]; int offset; - + offset = hair_grid_interp_weights(res, gmin, scale, vec, uvw); muvw[0] = 1.0f - uvw[0]; muvw[1] = 1.0f - uvw[1]; muvw[2] = 1.0f - uvw[2]; - + data[0] = grid[offset ]; data[1] = grid[offset +1]; data[2] = grid[offset +res[0] ]; @@ -145,14 +145,14 @@ BLI_INLINE void hair_grid_interpolate(const HairGridVert *grid, const int res[3] data[5] = grid[offset+res2 +1]; data[6] = grid[offset+res2+res[0] ]; data[7] = grid[offset+res2+res[0]+1]; - + if (density) { *density = muvw[2]*( muvw[1]*( muvw[0]*data[0].density + uvw[0]*data[1].density ) + uvw[1]*( muvw[0]*data[2].density + uvw[0]*data[3].density ) ) + uvw[2]*( muvw[1]*( muvw[0]*data[4].density + uvw[0]*data[5].density ) + uvw[1]*( muvw[0]*data[6].density + uvw[0]*data[7].density ) ); } - + if (velocity) { int k; for (k = 0; k < 3; ++k) { @@ -162,7 +162,7 @@ BLI_INLINE void hair_grid_interpolate(const HairGridVert *grid, const int res[3] uvw[1]*( muvw[0]*data[6].velocity[k] + uvw[0]*data[7].velocity[k] ) ); } } - + if (vel_smooth) { int k; for (k = 0; k < 3; ++k) { @@ -172,24 +172,24 @@ BLI_INLINE void hair_grid_interpolate(const HairGridVert *grid, const int res[3] uvw[1]*( muvw[0]*data[6].velocity_smooth[k] + uvw[0]*data[7].velocity_smooth[k] ) ); } } - + if (density_gradient) { density_gradient[0] = muvw[1] * muvw[2] * ( data[0].density - data[1].density ) + uvw[1] * muvw[2] * ( data[2].density - data[3].density ) + muvw[1] * uvw[2] * ( data[4].density - data[5].density ) + uvw[1] * uvw[2] * ( data[6].density - data[7].density ); - + density_gradient[1] = muvw[2] * muvw[0] * ( data[0].density - data[2].density ) + uvw[2] * muvw[0] * ( data[4].density - data[6].density ) + muvw[2] * uvw[0] * ( data[1].density - data[3].density ) + uvw[2] * uvw[0] * ( data[5].density - data[7].density ); - + density_gradient[2] = muvw[2] * muvw[0] * ( data[0].density - data[4].density ) + uvw[2] * muvw[0] * ( data[1].density - data[5].density ) + muvw[2] * uvw[0] * ( data[2].density - data[6].density ) + uvw[2] * uvw[0] * ( data[3].density - data[7].density ); } - + if (velocity_gradient) { /* XXX TODO */ zero_m3(velocity_gradient); @@ -201,21 +201,21 @@ void BPH_hair_volume_vertex_grid_forces(HairGrid *grid, const float x[3], const float f[3], float dfdx[3][3], float dfdv[3][3]) { float gdensity, gvelocity[3], ggrad[3], gvelgrad[3][3], gradlen; - + hair_grid_interpolate(grid->verts, grid->res, grid->gmin, grid->inv_cellsize, x, &gdensity, gvelocity, NULL, ggrad, gvelgrad); - + zero_v3(f); sub_v3_v3(gvelocity, v); mul_v3_v3fl(f, gvelocity, smoothfac); - + gradlen = normalize_v3(ggrad) - minpressure; if (gradlen > 0.0f) { mul_v3_fl(ggrad, gradlen); madd_v3_v3fl(f, ggrad, pressurefac); } - + zero_m3(dfdx); - + sub_m3_m3m3(dfdv, gvelgrad, I); mul_m3_fl(dfdv, smoothfac); } @@ -232,16 +232,16 @@ void BPH_hair_volume_grid_velocity(HairGrid *grid, const float x[3], const float { float gdensity, gvelocity[3], gvel_smooth[3], ggrad[3], gvelgrad[3][3]; float v_pic[3], v_flip[3]; - + hair_grid_interpolate(grid->verts, grid->res, grid->gmin, grid->inv_cellsize, x, &gdensity, gvelocity, gvel_smooth, ggrad, gvelgrad); - + /* velocity according to PIC method (Particle-in-Cell) */ copy_v3_v3(v_pic, gvel_smooth); - + /* velocity according to FLIP method (Fluid-Implicit-Particle) */ sub_v3_v3v3(v_flip, gvel_smooth, gvelocity); add_v3_v3(v_flip, v); - + interp_v3_v3v3(r_v, v_pic, v_flip, fluid_factor); } @@ -283,16 +283,16 @@ BLI_INLINE int hair_grid_weights(const int res[3], const float gmin[3], float sc { int i, j, k, offset; float uvw[3]; - + i = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 0); j = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 1); k = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 2); offset = i + (j + k*res[1])*res[0]; - + uvw[0] = (vec[0] - gmin[0]) * scale; uvw[1] = (vec[1] - gmin[1]) * scale; uvw[2] = (vec[2] - gmin[2]) * scale; - + weights[0] = dist_tent_v3f3(uvw, (float)i , (float)j , (float)k ); weights[1] = dist_tent_v3f3(uvw, (float)(i+1), (float)j , (float)k ); weights[2] = dist_tent_v3f3(uvw, (float)i , (float)(j+1), (float)k ); @@ -301,9 +301,9 @@ BLI_INLINE int hair_grid_weights(const int res[3], const float gmin[3], float sc weights[5] = dist_tent_v3f3(uvw, (float)(i+1), (float)j , (float)(k+1)); weights[6] = dist_tent_v3f3(uvw, (float)i , (float)(j+1), (float)(k+1)); weights[7] = dist_tent_v3f3(uvw, (float)(i+1), (float)(j+1), (float)(k+1)); - + // BLI_assert(fabsf(weights_sum(weights) - 1.0f) < 0.0001f); - + return offset; } @@ -320,18 +320,18 @@ void BPH_hair_volume_add_vertex(HairGrid *grid, const float x[3], const float v[ float weights[8]; int di, dj, dk; int offset; - + if (!hair_grid_point_valid(x, grid->gmin, grid->gmax)) return; - + offset = hair_grid_weights(res, grid->gmin, grid->inv_cellsize, x, weights); - + for (di = 0; di < 2; ++di) { for (dj = 0; dj < 2; ++dj) { for (dk = 0; dk < 2; ++dk) { int voffset = offset + di + (dj + dk*res[1])*res[0]; int iw = di + dj*2 + dk*4; - + grid->verts[voffset].density += weights[iw]; madd_v3_v3fl(grid->verts[voffset].velocity, v, weights[iw]); } @@ -344,15 +344,15 @@ BLI_INLINE void hair_volume_eval_grid_vertex(HairGridVert *vert, const float loc const float x2[3], const float v2[3], const float x3[3], const float v3[3]) { float closest[3], lambda, dist, weight; - + lambda = closest_to_line_v3(closest, loc, x2, x3); dist = len_v3v3(closest, loc); - + weight = (radius - dist) * dist_scale; - + if (weight > 0.0f) { float vel[3]; - + interp_v3_v3v3(vel, v2, v3, lambda); madd_v3_v3fl(vert->velocity, vel, weight); vert->density += weight; @@ -378,37 +378,37 @@ BLI_INLINE void hair_volume_add_segment_2D(HairGrid *grid, { const float radius = 1.5f; const float dist_scale = grid->inv_cellsize; - + int j, k; - + /* boundary checks to be safe */ CLAMP_MIN(jmin, 0); CLAMP_MAX(jmax, resj-1); CLAMP_MIN(kmin, 0); CLAMP_MAX(kmax, resk-1); - + HairGridVert *vert_j = vert + jmin * stride_j; float loc_j[3] = { loc[0], loc[1], loc[2] }; loc_j[axis_j] += (float)jmin; for (j = jmin; j <= jmax; ++j, vert_j += stride_j, loc_j[axis_j] += 1.0f) { - + HairGridVert *vert_k = vert_j + kmin * stride_k; float loc_k[3] = { loc_j[0], loc_j[1], loc_j[2] }; loc_k[axis_k] += (float)kmin; for (k = kmin; k <= kmax; ++k, vert_k += stride_k, loc_k[axis_k] += 1.0f) { - + hair_volume_eval_grid_vertex(vert_k, loc_k, radius, dist_scale, x2, v2, x3, v3); - + #if 0 { float wloc[3], x2w[3], x3w[3]; grid_to_world(grid, wloc, loc_k); grid_to_world(grid, x2w, x2); grid_to_world(grid, x3w, x3); - + if (vert_k->samples > 0) BKE_sim_debug_data_add_circle(wloc, 0.01f, 1.0, 1.0, 0.3, "grid", 2525, debug_i, j, k); - + if (grid->debug_value) { BKE_sim_debug_data_add_dot(wloc, 1, 0, 0, "grid", 93, debug_i, j, k); BKE_sim_debug_data_add_dot(x2w, 0.1, 0.1, 0.7, "grid", 649, debug_i, j, k); @@ -435,55 +435,55 @@ void BPH_hair_volume_add_segment(HairGrid *grid, const float dir1[3], const float dir2[3], const float dir3[3]) { const int res[3] = { grid->res[0], grid->res[1], grid->res[2] }; - + /* find the primary direction from the major axis of the direction vector */ const int axis0 = major_axis_v3(dir2); const int axis1 = (axis0 + 1) % 3; const int axis2 = (axis0 + 2) % 3; - + /* vertex buffer offset factors along cardinal axes */ const int strides[3] = { 1, res[0], res[0] * res[1] }; const int stride0 = strides[axis0]; const int stride1 = strides[axis1]; const int stride2 = strides[axis2]; - + /* increment of secondary directions per step in the primary direction * note: we always go in the positive direction along axis0, so the sign can be inverted */ const float inc1 = dir2[axis1] / dir2[axis0]; const float inc2 = dir2[axis2] / dir2[axis0]; - + /* start/end points, so increment along axis0 is always positive */ const float *start = x2[axis0] < x3[axis0] ? x2 : x3; const float *end = x2[axis0] < x3[axis0] ? x3 : x2; const float start0 = start[axis0], start1 = start[axis1], start2 = start[axis2]; const float end0 = end[axis0]; - + /* range along primary direction */ const int imin = max_ii(floor_int(start[axis0]) - 1, 0); const int imax = min_ii(floor_int(end[axis0]) + 2, res[axis0]-1); - + float h = 0.0f; HairGridVert *vert0; float loc0[3]; int j0, k0, j0_prev, k0_prev; int i; - + for (i = imin; i <= imax; ++i) { float shift1, shift2; /* fraction of a full cell shift [0.0, 1.0) */ int jmin, jmax, kmin, kmax; - + h = CLAMPIS((float)i, start0, end0); - + shift1 = start1 + (h - start0) * inc1; shift2 = start2 + (h - start0) * inc2; - + j0_prev = j0; j0 = floor_int(shift1); - + k0_prev = k0; k0 = floor_int(shift2); - + if (i > imin) { jmin = min_ii(j0, j0_prev); jmax = max_ii(j0, j0_prev); @@ -494,12 +494,12 @@ void BPH_hair_volume_add_segment(HairGrid *grid, jmin = jmax = j0; kmin = kmax = k0; } - + vert0 = grid->verts + i * stride0; loc0[axis0] = (float)i; loc0[axis1] = 0.0f; loc0[axis2] = 0.0f; - + hair_volume_add_segment_2D(grid, x1, v1, x2, v2, x3, v3, x4, v4, dir1, dir2, dir3, res[axis1], res[axis2], jmin-1, jmax+2, kmin-1, kmax+2, vert0, stride1, stride2, loc0, axis1, axis2, @@ -511,11 +511,11 @@ BLI_INLINE void hair_volume_eval_grid_vertex_sample(HairGridVert *vert, const fl const float x[3], const float v[3]) { float dist, weight; - + dist = len_v3v3(x, loc); - + weight = (radius - dist) * dist_scale; - + if (weight > 0.0f) { madd_v3_v3fl(vert->velocity, v, weight); vert->density += weight; @@ -533,34 +533,34 @@ void BPH_hair_volume_add_segment(HairGrid *grid, { const float radius = 1.5f; const float dist_scale = grid->inv_cellsize; - + const int res[3] = { grid->res[0], grid->res[1], grid->res[2] }; const int stride[3] = { 1, res[0], res[0] * res[1] }; const int num_samples = 10; - + int s; - + for (s = 0; s < num_samples; ++s) { float x[3], v[3]; int i, j, k; - + float f = (float)s / (float)(num_samples-1); interp_v3_v3v3(x, x2, x3, f); interp_v3_v3v3(v, v2, v3, f); - + int imin = max_ii(floor_int(x[0]) - 2, 0); int imax = min_ii(floor_int(x[0]) + 2, res[0]-1); int jmin = max_ii(floor_int(x[1]) - 2, 0); int jmax = min_ii(floor_int(x[1]) + 2, res[1]-1); int kmin = max_ii(floor_int(x[2]) - 2, 0); int kmax = min_ii(floor_int(x[2]) + 2, res[2]-1); - + for (k = kmin; k <= kmax; ++k) { for (j = jmin; j <= jmax; ++j) { for (i = imin; i <= imax; ++i) { float loc[3] = { (float)i, (float)j, (float)k }; HairGridVert *vert = grid->verts + i * stride[0] + j * stride[1] + k * stride[2]; - + hair_volume_eval_grid_vertex_sample(vert, loc, radius, dist_scale, x, v); } } @@ -598,25 +598,25 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target { const float flowfac = grid->cellsize; const float inv_flowfac = 1.0f / grid->cellsize; - + /*const int num_cells = hair_grid_size(grid->res);*/ const int res[3] = { grid->res[0], grid->res[1], grid->res[2] }; const int resA[3] = { grid->res[0] + 2, grid->res[1] + 2, grid->res[2] + 2 }; - + const int stride0 = 1; const int stride1 = grid->res[0]; const int stride2 = grid->res[1] * grid->res[0]; const int strideA0 = 1; const int strideA1 = grid->res[0] + 2; const int strideA2 = (grid->res[1] + 2) * (grid->res[0] + 2); - + const int num_cells = res[0] * res[1] * res[2]; const int num_cellsA = (res[0] + 2) * (res[1] + 2) * (res[2] + 2); - + HairGridVert *vert_start = grid->verts - (stride0 + stride1 + stride2); HairGridVert *vert; int i, j, k; - + #define MARGIN_i0 (i < 1) #define MARGIN_j0 (j < 1) #define MARGIN_k0 (k < 1) @@ -630,9 +630,9 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target #define NEIGHBOR_MARGIN_i1 (i >= resA[0]-2) #define NEIGHBOR_MARGIN_j1 (j >= resA[1]-2) #define NEIGHBOR_MARGIN_k1 (k >= resA[2]-2) - + BLI_assert(num_cells >= 1); - + /* Calculate divergence */ lVector B(num_cellsA); for (k = 0; k < resA[2]; ++k) { @@ -640,14 +640,14 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target for (i = 0; i < resA[0]; ++i) { int u = i * strideA0 + j * strideA1 + k * strideA2; bool is_margin = MARGIN_i0 || MARGIN_i1 || MARGIN_j0 || MARGIN_j1 || MARGIN_k0 || MARGIN_k1; - + if (is_margin) { B[u] = 0.0f; continue; } - + vert = vert_start + i * stride0 + j * stride1 + k * stride2; - + const float *v0 = vert->velocity; float dx = 0.0f, dy = 0.0f, dz = 0.0f; if (!NEIGHBOR_MARGIN_i0) @@ -662,19 +662,19 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target dz += v0[2] - (vert - stride2)->velocity[2]; if (!NEIGHBOR_MARGIN_k1) dz += (vert + stride2)->velocity[2] - v0[2]; - + float divergence = -0.5f * flowfac * (dx + dy + dz); - + /* adjustment term for target density */ float target = hair_volume_density_divergence(vert->density, target_density, target_strength); - + /* B vector contains the finite difference approximation of the velocity divergence. * Note: according to the discretized Navier-Stokes equation the rhs vector * and resulting pressure gradient should be multiplied by the (inverse) density; * however, this is already included in the weighting of hair velocities on the grid! */ B[u] = divergence - target; - + #if 0 { float wloc[3], loc[3]; @@ -683,12 +683,12 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target float coln[3] = {1.0, 0.0, 1.0}; float col[3]; float fac; - + loc[0] = (float)(i - 1); loc[1] = (float)(j - 1); loc[2] = (float)(k - 1); grid_to_world(grid, wloc, loc); - + if (divergence > 0.0f) { fac = CLAMPIS(divergence * target_strength, 0.0, 1.0); interp_v3_v3v3(col, col0, colp, fac); @@ -704,11 +704,11 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target } } } - + /* Main Poisson equation system: * This is derived from the discretezation of the Poisson equation * div(grad(p)) = div(v) - * + * * The finite difference approximation yields the linear equation system described here: * https://en.wikipedia.org/wiki/Discrete_Poisson_equation */ @@ -718,13 +718,13 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target * and up to 6 factors -1 on other places. */ A.reserve(Eigen::VectorXi::Constant(num_cellsA, 7)); - + for (k = 0; k < resA[2]; ++k) { for (j = 0; j < resA[1]; ++j) { for (i = 0; i < resA[0]; ++i) { int u = i * strideA0 + j * strideA1 + k * strideA2; bool is_margin = MARGIN_i0 || MARGIN_i1 || MARGIN_j0 || MARGIN_j1 || MARGIN_k0 || MARGIN_k1; - + vert = vert_start + i * stride0 + j * stride1 + k * stride2; if (!is_margin && vert->density > density_threshold) { int neighbors_lo = 0; @@ -733,7 +733,7 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target int neighbor_lo_index[3]; int neighbor_hi_index[3]; int n; - + /* check for upper bounds in advance * to get the correct number of neighbors, * needed for the diagonal element @@ -750,10 +750,10 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target neighbor_hi_index[neighbors_hi++] = u + strideA1; if (!NEIGHBOR_MARGIN_k1 && (vert + stride2)->density > density_threshold) neighbor_hi_index[neighbors_hi++] = u + strideA2; - + /*int liquid_neighbors = neighbors_lo + neighbors_hi;*/ non_solid_neighbors = 6; - + for (n = 0; n < neighbors_lo; ++n) A.insert(neighbor_lo_index[n], u) = -1.0f; A.insert(u, u) = (float)non_solid_neighbors; @@ -766,15 +766,15 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target } } } - + ConjugateGradient cg; cg.setMaxIterations(100); cg.setTolerance(0.01f); - + cg.compute(A); - + lVector p = cg.solve(B); - + if (cg.info() == Eigen::Success) { /* Calculate velocity = grad(p) */ for (k = 0; k < resA[2]; ++k) { @@ -784,7 +784,7 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target bool is_margin = MARGIN_i0 || MARGIN_i1 || MARGIN_j0 || MARGIN_j1 || MARGIN_k0 || MARGIN_k1; if (is_margin) continue; - + vert = vert_start + i * stride0 + j * stride1 + k * stride2; if (vert->density > density_threshold) { float p_left = p[u - strideA0]; @@ -793,14 +793,14 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target float p_up = p[u + strideA1]; float p_bottom = p[u - strideA2]; float p_top = p[u + strideA2]; - + /* finite difference estimate of pressure gradient */ float dvel[3]; dvel[0] = p_right - p_left; dvel[1] = p_up - p_down; dvel[2] = p_top - p_bottom; mul_v3_fl(dvel, -0.5f * inv_flowfac); - + /* pressure gradient describes velocity delta */ add_v3_v3v3(vert->velocity_smooth, vert->velocity, dvel); } @@ -810,14 +810,14 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target } } } - + #if 0 { int axis = 0; float offset = 0.0f; - + int slice = (offset - grid->gmin[axis]) / grid->cellsize; - + for (k = 0; k < resA[2]; ++k) { for (j = 0; j < resA[1]; ++j) { for (i = 0; i < resA[0]; ++i) { @@ -825,21 +825,21 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target bool is_margin = MARGIN_i0 || MARGIN_i1 || MARGIN_j0 || MARGIN_j1 || MARGIN_k0 || MARGIN_k1; if (i != slice) continue; - + vert = vert_start + i * stride0 + j * stride1 + k * stride2; - + float wloc[3], loc[3]; float col0[3] = {0.0, 0.0, 0.0}; float colp[3] = {0.0, 1.0, 1.0}; float coln[3] = {1.0, 0.0, 1.0}; float col[3]; float fac; - + loc[0] = (float)(i - 1); loc[1] = (float)(j - 1); loc[2] = (float)(k - 1); grid_to_world(grid, wloc, loc); - + float pressure = p[u]; if (pressure > 0.0f) { fac = CLAMPIS(pressure * grid->debug1, 0.0, 1.0); @@ -851,19 +851,19 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target } if (fac > 0.05f) BKE_sim_debug_data_add_circle(grid->debug_data, wloc, 0.01f, col[0], col[1], col[2], "grid", 5533, i, j, k); - + if (!is_margin) { float dvel[3]; sub_v3_v3v3(dvel, vert->velocity_smooth, vert->velocity); // BKE_sim_debug_data_add_vector(grid->debug_data, wloc, dvel, 1, 1, 1, "grid", 5566, i, j, k); } - + if (!is_margin) { float d = CLAMPIS(vert->density * grid->debug2, 0.0f, 1.0f); float col0[3] = {0.3, 0.3, 0.3}; float colp[3] = {0.0, 0.0, 1.0}; float col[3]; - + interp_v3_v3v3(col, col0, colp, d); // if (d > 0.05f) // BKE_sim_debug_data_add_dot(grid->debug_data, wloc, col[0], col[1], col[2], "grid", 5544, i, j, k); @@ -873,7 +873,7 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target } } #endif - + return true; } else { @@ -881,7 +881,7 @@ bool BPH_hair_volume_solve_divergence(HairGrid *grid, float /*dt*/, float target for (i = 0, vert = grid->verts; i < num_cells; ++i, ++vert) { zero_v3(vert->velocity_smooth); } - + return false; } } @@ -901,20 +901,20 @@ BLI_INLINE void hair_volume_filter_box_convolute(HairVertexGrid *grid, float inv int offset, kernel_offset, kernel_dq, kernel_dr; HairGridVert *verts; float *vel_smooth; - + offset = i + (j + k*res)*res; verts = grid->verts; vel_smooth = verts[offset].velocity_smooth; - + kernel_offset = minp + (minq + minr*res)*res; kernel_dq = res; kernel_dr = res * res; for (r = minr; r <= maxr; ++r) { for (q = minq; q <= maxq; ++q) { for (p = minp; p <= maxp; ++p) { - + madd_v3_v3fl(vel_smooth, verts[kernel_offset].velocity, invD); - + kernel_offset += 1; } kernel_offset += kernel_dq; @@ -930,18 +930,18 @@ void BPH_hair_volume_vertex_grid_filter_box(HairVertexGrid *grid, int kernel_siz int tot; float invD; int i, j, k; - + if (kernel_size <= 0) return; - + tot = kernel_size * 2 + 1; invD = 1.0f / (float)(tot*tot*tot); - + /* clear values for convolution */ for (i = 0; i < size; ++i) { zero_v3(grid->verts[i].velocity_smooth); } - + for (i = 0; i < grid->res; ++i) { for (j = 0; j < grid->res; ++j) { for (k = 0; k < grid->res; ++k) { @@ -949,7 +949,7 @@ void BPH_hair_volume_vertex_grid_filter_box(HairVertexGrid *grid, int kernel_siz } } } - + /* apply as new velocity */ for (i = 0; i < size; ++i) { copy_v3_v3(grid->verts[i].velocity, grid->verts[i].velocity_smooth); @@ -966,21 +966,21 @@ HairGrid *BPH_hair_volume_create_vertex_grid(float cellsize, const float gmin[3] int size; HairGrid *grid; int i; - + /* sanity check */ if (cellsize <= 0.0f) cellsize = 1.0f; scale = 1.0f / cellsize; - + sub_v3_v3v3(extent, gmax, gmin); for (i = 0; i < 3; ++i) { resmin[i] = floor_int(gmin[i] * scale); resmax[i] = floor_int(gmax[i] * scale) + 1; - + /* add margin of 1 cell */ resmin[i] -= 1; resmax[i] += 1; - + res[i] = resmax[i] - resmin[i] + 1; /* sanity check: avoid null-sized grid */ if (res[i] < 4) { @@ -992,12 +992,12 @@ HairGrid *BPH_hair_volume_create_vertex_grid(float cellsize, const float gmin[3] res[i] = MAX_HAIR_GRID_RES; resmax[i] = resmin[i] + MAX_HAIR_GRID_RES; } - + gmin_margin[i] = (float)resmin[i] * cellsize; gmax_margin[i] = (float)resmax[i] * cellsize; } size = hair_grid_size(res); - + grid = (HairGrid *)MEM_callocN(sizeof(HairGrid), "hair grid"); grid->res[0] = res[0]; grid->res[1] = res[1]; @@ -1062,23 +1062,23 @@ static HairGridVert *hair_volume_create_collision_grid(ClothModifierData *clmd, float vel[3]; float weights[8]; int di, dj, dk; - + for (v=0; v < col->collmd->numverts; v++, loc0++, loc1++) { int offset; - + if (!hair_grid_point_valid(loc1->co, gmin, gmax)) continue; - + offset = hair_grid_weights(res, gmin, scale, lX[v], weights); - + sub_v3_v3v3(vel, loc1->co, loc0->co); - + for (di = 0; di < 2; ++di) { for (dj = 0; dj < 2; ++dj) { for (dk = 0; dk < 2; ++dk) { int voffset = offset + di + (dj + dk*res)*res; int iw = di + dj*2 + dk*4; - + collgrid[voffset].density += weights[iw]; madd_v3_v3fl(collgrid[voffset].velocity, vel, weights[iw]); } @@ -1095,7 +1095,7 @@ static HairGridVert *hair_volume_create_collision_grid(ClothModifierData *clmd, if (density > 0.0f) mul_v3_fl(collgrid[i].velocity, 1.0f/density); } - + return collgrid; } #endif @@ -1108,9 +1108,9 @@ bool BPH_hair_volume_get_texture_data(HairGrid *grid, VoxelData *vd) vd->resol[0] = grid->res[0]; vd->resol[1] = grid->res[1]; vd->resol[2] = grid->res[2]; - + totres = hair_grid_size(grid->res); - + if (vd->hair_type == TEX_VD_HAIRVELOCITY) { depth = 4; vd->data_type = TEX_VD_RGBA_PREMUL; @@ -1119,20 +1119,20 @@ bool BPH_hair_volume_get_texture_data(HairGrid *grid, VoxelData *vd) depth = 1; vd->data_type = TEX_VD_INTENSITY; } - + if (totres > 0) { vd->dataset = (float *)MEM_mapallocN(sizeof(float) * depth * (totres), "hair volume texture data"); - + for (i = 0; i < totres; ++i) { switch (vd->hair_type) { case TEX_VD_HAIRDENSITY: vd->dataset[i] = grid->verts[i].density; break; - + case TEX_VD_HAIRRESTDENSITY: vd->dataset[i] = 0.0f; // TODO break; - + case TEX_VD_HAIRVELOCITY: { vd->dataset[i + 0*totres] = grid->verts[i].velocity[0]; vd->dataset[i + 1*totres] = grid->verts[i].velocity[1]; @@ -1149,6 +1149,6 @@ bool BPH_hair_volume_get_texture_data(HairGrid *grid, VoxelData *vd) else { vd->dataset = NULL; } - + return true; } diff --git a/source/blender/physics/intern/implicit.h b/source/blender/physics/intern/implicit.h index 2f62ab98e12..e2ac6c306b6 100644 --- a/source/blender/physics/intern/implicit.h +++ b/source/blender/physics/intern/implicit.h @@ -62,7 +62,7 @@ struct Implicit_Data; typedef struct ImplicitSolverResult { int status; - + int iterations; float error; } ImplicitSolverResult; diff --git a/source/blender/physics/intern/implicit_blender.c b/source/blender/physics/intern/implicit_blender.c index 49798081cff..5fd9c6b50de 100644 --- a/source/blender/physics/intern/implicit_blender.c +++ b/source/blender/physics/intern/implicit_blender.c @@ -71,9 +71,9 @@ static float ZERO[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; /* #define C99 #ifdef C99 -#defineDO_INLINE inline -#else -#defineDO_INLINE static +#defineDO_INLINE inline +#else +#defineDO_INLINE static #endif */ struct Cloth; @@ -90,7 +90,7 @@ typedef struct fmatrix3x3 { /* int pinned; // is this vertex allowed to move? */ float n1, n2, n3; /* three normal vectors for collision constrains */ unsigned int vcount; /* vertex count */ - unsigned int scount; /* spring count */ + unsigned int scount; /* spring count */ } fmatrix3x3; /////////////////////////// @@ -115,9 +115,9 @@ DO_INLINE void mul_fvectorT_fvector(float to[3][3], float vectorA[3], float vect /* simple v^T * v product with scalar ("outer product") */ /* STATUS: HAS TO BE verified (*should* work) */ DO_INLINE void mul_fvectorT_fvectorS(float to[3][3], float vectorA[3], float vectorB[3], float aS) -{ +{ mul_fvectorT_fvector(to, vectorA, vectorB); - + mul_fvector_S(to[0], to[0], aS); mul_fvector_S(to[1], to[1], aS); mul_fvector_S(to[2], to[2], aS); @@ -288,7 +288,7 @@ static void print_lvector(lfVector *v, int numverts) for (i = 0; i < numverts; ++i) { if (i > 0) printf("\n"); - + printf("%f,\n", v[i][0]); printf("%f,\n", v[i][1]); printf("%f,\n", v[i][2]); @@ -303,11 +303,11 @@ static void print_bfmatrix(fmatrix3x3 *m) int size = m[0].vcount * 3; float *t = MEM_callocN(sizeof(float) * size*size, "bfmatrix"); int q, i, j; - + for (q = 0; q < tot; ++q) { int k = 3 * m[q].r; int l = 3 * m[q].c; - + for (j = 0; j < 3; ++j) { for (i = 0; i < 3; ++i) { // if (t[k + i + (l + j) * size] != 0.0f) { @@ -323,20 +323,20 @@ static void print_bfmatrix(fmatrix3x3 *m) } } } - + for (j = 0; j < size; ++j) { if (j > 0 && j % 3 == 0) printf("\n"); - + for (i = 0; i < size; ++i) { if (i > 0 && i % 3 == 0) printf(" "); - + implicit_print_matrix_elem(t[i + j * size]); } printf("\n"); } - + MEM_freeN(t); } #endif @@ -354,7 +354,7 @@ DO_INLINE void cp_fmatrix(float to[3][3], float from[3][3]) DO_INLINE void initdiag_fmatrixS(float to[3][3], float aS) { cp_fmatrix(to, ZERO); - + to[0][0] = aS; to[1][1] = aS; to[2][2] = aS; @@ -532,15 +532,15 @@ DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs) // TODO: check if memory allocation was successful */ fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN(sizeof(fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix"); int i; - + temp[0].vcount = verts; temp[0].scount = springs; - + /* vertex part of the matrix is diagonal blocks */ for (i = 0; i < verts; ++i) { init_fmatrix(temp + i, i, i); } - + return temp; } /* delete big matrix */ @@ -565,7 +565,7 @@ DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][3]) unsigned int i; for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) { - cp_fmatrix(matrix[i].m, m3); + cp_fmatrix(matrix[i].m, m3); } } @@ -577,10 +577,10 @@ DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][3]) float tmatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; for (i = 0; i < matrix[0].vcount; i++) { - cp_fmatrix(matrix[i].m, m3); + cp_fmatrix(matrix[i].m, m3); } for (j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++) { - cp_fmatrix(matrix[j].m, tmatrix); + cp_fmatrix(matrix[j].m, tmatrix); } } @@ -591,7 +591,7 @@ DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector unsigned int i = 0; unsigned int vcount = from[0].vcount; lfVector *temp = create_lfvector(vcount); - + zero_lfvector(to, vcount); #pragma omp parallel sections private(i) if (vcount > CLOTH_OPENMP_LIMIT) @@ -610,10 +610,10 @@ DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector } } add_lfvector_lfvector(to, to, temp, from[0].vcount); - + del_lfvector(temp); - - + + } /* SPARSE SYMMETRIC sub big matrix with big matrix*/ @@ -642,15 +642,15 @@ typedef struct Implicit_Data { lfVector *F; /* forces */ fmatrix3x3 *dFdV, *dFdX; /* force jacobians */ int num_blocks; /* number of off-diagonal blocks (springs) */ - + /* motion state data */ lfVector *X, *Xnew; /* positions */ lfVector *V, *Vnew; /* velocities */ - + /* internal solver data */ lfVector *B; /* B for A*dV = B */ fmatrix3x3 *A; /* A for A*dV = B */ - + lfVector *dV; /* velocity change (solution of A*dV = B) */ lfVector *z; /* target velocity in constrained directions */ fmatrix3x3 *S; /* filtering matrix for constraints */ @@ -660,7 +660,7 @@ typedef struct Implicit_Data { Implicit_Data *BPH_mass_spring_solver_create(int numverts, int numsprings) { Implicit_Data *id = (Implicit_Data *)MEM_callocN(sizeof(Implicit_Data), "implicit vecmat"); - + /* process diagonal elements */ id->tfm = create_bfmatrix(numverts, 0); id->A = create_bfmatrix(numverts, numsprings); @@ -696,7 +696,7 @@ void BPH_mass_spring_solver_free(Implicit_Data *id) del_bfmatrix(id->Pinv); del_bfmatrix(id->bigI); del_bfmatrix(id->M); - + del_lfvector(id->X); del_lfvector(id->Xnew); del_lfvector(id->V); @@ -705,7 +705,7 @@ void BPH_mass_spring_solver_free(Implicit_Data *id) del_lfvector(id->B); del_lfvector(id->dV); del_lfvector(id->z); - + MEM_freeN(id); } @@ -752,7 +752,7 @@ static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z // Solves for unknown X in equation AX=B unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100; float conjgrad_epsilon=0.0001f /* , conjgrad_lasterror=0 */ /* UNUSED */; - lfVector *q, *d, *tmp, *r; + lfVector *q, *d, *tmp, *r; float s, starget, a, s_prev; unsigned int numverts = lA[0].vcount; q = create_lfvector(numverts); @@ -818,7 +818,7 @@ static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, // Solves for unknown X in equation AX=B unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100; float conjgrad_epsilon=0.01f; - + unsigned int numverts = lA[0].vcount; lfVector *fB = create_lfvector(numverts); lfVector *AdV = create_lfvector(numverts); @@ -827,27 +827,27 @@ static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, lfVector *q = create_lfvector(numverts); lfVector *s = create_lfvector(numverts); float bnorm2, delta_new, delta_old, delta_target, alpha; - + cp_lfvector(ldV, z, numverts); - + /* d0 = filter(B)^T * P * filter(B) */ cp_lfvector(fB, lB, numverts); filter(fB, S); bnorm2 = dot_lfvector(fB, fB, numverts); delta_target = conjgrad_epsilon*conjgrad_epsilon * bnorm2; - + /* r = filter(B - A * dV) */ mul_bfmatrix_lfvector(AdV, lA, ldV); sub_lfvector_lfvector(r, lB, AdV, numverts); filter(r, S); - + /* c = filter(P^-1 * r) */ cp_lfvector(c, r, numverts); filter(c, S); - + /* delta = r^T * c */ delta_new = dot_lfvector(r, c, numverts); - + #ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT printf("==== A ====\n"); print_bfmatrix(lA); @@ -858,25 +858,25 @@ static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, printf("==== S ====\n"); print_bfmatrix(S); #endif - + while (delta_new > delta_target && conjgrad_loopcount < conjgrad_looplimit) { mul_bfmatrix_lfvector(q, lA, c); filter(q, S); - + alpha = delta_new / dot_lfvector(c, q, numverts); - + add_lfvector_lfvectorS(ldV, ldV, c, alpha, numverts); - + add_lfvector_lfvectorS(r, r, q, -alpha, numverts); - + /* s = P^-1 * r */ cp_lfvector(s, r, numverts); delta_old = delta_new; delta_new = dot_lfvector(r, s, numverts); - + add_lfvector_lfvectorS(c, s, c, delta_new / delta_old, numverts); filter(c, S); - + conjgrad_loopcount++; } @@ -885,7 +885,7 @@ static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, print_lvector(ldV, numverts); printf("========\n"); #endif - + del_lfvector(fB); del_lfvector(AdV); del_lfvector(r); @@ -906,14 +906,14 @@ static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv) { unsigned int i = 0; - + // Take only the diagonal blocks of A // #pragma omp parallel for private(i) if (lA[0].vcount > CLOTH_OPENMP_LIMIT) for (i = 0; i<lA[0].vcount; i++) { // block diagonalizer cp_fmatrix(P[i].m, lA[i].m); inverse_fmatrix(Pinv[i].m, P[i].m); - + } } /* @@ -927,65 +927,65 @@ static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector lfVector *p = create_lfvector(numverts); lfVector *s = create_lfvector(numverts); lfVector *h = create_lfvector(numverts); - + BuildPPinv(lA, P, Pinv); - + filter(dv, S); add_lfvector_lfvector(dv, dv, z, numverts); - + mul_bfmatrix_lfvector(r, lA, dv); sub_lfvector_lfvector(r, lB, r, numverts); filter(r, S); - + mul_prevfmatrix_lfvector(p, Pinv, r); filter(p, S); - + deltaNew = dot_lfvector(r, p, numverts); - + delta0 = deltaNew * sqrt(conjgrad_epsilon); - + #ifdef DEBUG_TIME double start = PIL_check_seconds_timer(); #endif - + while ((deltaNew > delta0) && (iterations < conjgrad_looplimit)) { iterations++; - + mul_bfmatrix_lfvector(s, lA, p); filter(s, S); - + alpha = deltaNew / dot_lfvector(p, s, numverts); - + add_lfvector_lfvectorS(dv, dv, p, alpha, numverts); - + add_lfvector_lfvectorS(r, r, s, -alpha, numverts); - + mul_prevfmatrix_lfvector(h, Pinv, r); filter(h, S); - + deltaOld = deltaNew; - + deltaNew = dot_lfvector(r, h, numverts); - + add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts); - + filter(p, S); - + } #ifdef DEBUG_TIME double end = PIL_check_seconds_timer(); printf("cg_filtered_pre time: %f\n", (float)(end - start)); #endif - + del_lfvector(h); del_lfvector(s); del_lfvector(p); del_lfvector(r); - + printf("iterations: %d\n", iterations); - + return iterations<conjgrad_looplimit; } */ @@ -1000,83 +1000,83 @@ static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector lfVector *h = create_lfvector(numverts); lfVector *bhat = create_lfvector(numverts); lfVector *btemp = create_lfvector(numverts); - + BuildPPinv(lA, P, Pinv); - + initdiag_bfmatrix(bigI, I); sub_bfmatrix_Smatrix(bigI, bigI, S); - + // x = Sx_0+(I-S)z filter(dv, S); add_lfvector_lfvector(dv, dv, z, numverts); - + // b_hat = S(b-A(I-S)z) mul_bfmatrix_lfvector(r, lA, z); mul_bfmatrix_lfvector(bhat, bigI, r); sub_lfvector_lfvector(bhat, lB, bhat, numverts); - + // r = S(b-Ax) mul_bfmatrix_lfvector(r, lA, dv); sub_lfvector_lfvector(r, lB, r, numverts); filter(r, S); - + // p = SP^-1r mul_prevfmatrix_lfvector(p, Pinv, r); filter(p, S); - + // delta0 = bhat^TP^-1bhat mul_prevfmatrix_lfvector(btemp, Pinv, bhat); delta0 = dot_lfvector(bhat, btemp, numverts); - + // deltaNew = r^TP deltaNew = dot_lfvector(r, p, numverts); - + /* filter(dv, S); add_lfvector_lfvector(dv, dv, z, numverts); - + mul_bfmatrix_lfvector(r, lA, dv); sub_lfvector_lfvector(r, lB, r, numverts); filter(r, S); - + mul_prevfmatrix_lfvector(p, Pinv, r); filter(p, S); - + deltaNew = dot_lfvector(r, p, numverts); - + delta0 = deltaNew * sqrt(conjgrad_epsilon); */ #ifdef DEBUG_TIME double start = PIL_check_seconds_timer(); #endif - + tol = (0.01*0.2); - + while ((deltaNew > delta0*tol*tol) && (iterations < conjgrad_looplimit)) { iterations++; - + mul_bfmatrix_lfvector(s, lA, p); filter(s, S); - + alpha = deltaNew / dot_lfvector(p, s, numverts); - + add_lfvector_lfvectorS(dv, dv, p, alpha, numverts); - + add_lfvector_lfvectorS(r, r, s, -alpha, numverts); - + mul_prevfmatrix_lfvector(h, Pinv, r); filter(h, S); - + deltaOld = deltaNew; - + deltaNew = dot_lfvector(r, h, numverts); - + add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts); - + filter(p, S); - + } #ifdef DEBUG_TIME @@ -1090,9 +1090,9 @@ static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector del_lfvector(s); del_lfvector(p); del_lfvector(r); - + // printf("iterations: %d\n", iterations); - + return iterations<conjgrad_looplimit; } #endif @@ -1128,17 +1128,17 @@ bool BPH_mass_spring_solve_velocities(Implicit_Data *data, float dt, ImplicitSol add_lfvector_lfvector(data->Vnew, data->V, data->dV, numverts); del_lfvector(dFdXmV); - + return result->status == BPH_SOLVER_SUCCESS; } bool BPH_mass_spring_solve_positions(Implicit_Data *data, float dt) { int numverts = data->M[0].vcount; - + // advance positions add_lfvector_lfvectorS(data->Xnew, data->X, data->Vnew, dt, numverts); - + return true; } @@ -1219,7 +1219,7 @@ static int BPH_mass_spring_add_block(Implicit_Data *data, int v1, int v2) int s = data->M[0].vcount + data->num_blocks; /* index from array start */ BLI_assert(s < data->M[0].vcount + data->M[0].scount); ++data->num_blocks; - + /* tfm and S don't have spring entries (diagonal blocks only) */ init_fmatrix(data->bigI + s, v1, v2); init_fmatrix(data->M + s, v1, v2); @@ -1228,7 +1228,7 @@ static int BPH_mass_spring_add_block(Implicit_Data *data, int v1, int v2) init_fmatrix(data->A + s, v1, v2); init_fmatrix(data->P + s, v1, v2); init_fmatrix(data->Pinv + s, v1, v2); - + return s; } @@ -1244,26 +1244,26 @@ void BPH_mass_spring_clear_constraints(Implicit_Data *data) void BPH_mass_spring_add_constraint_ndof0(Implicit_Data *data, int index, const float dV[3]) { zero_m3(data->S[index].m); - + world_to_root_v3(data, index, data->z[index], dV); } void BPH_mass_spring_add_constraint_ndof1(Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3]) { float m[3][3], p[3], q[3], u[3], cmat[3][3]; - + world_to_root_v3(data, index, p, c1); mul_fvectorT_fvector(cmat, p, p); sub_m3_m3m3(m, I, cmat); - + world_to_root_v3(data, index, q, c2); mul_fvectorT_fvector(cmat, q, q); sub_m3_m3m3(m, m, cmat); - + /* XXX not sure but multiplication should work here */ copy_m3_m3(data->S[index].m, m); // mul_m3_m3m3(data->S[index].m, data->S[index].m, m); - + world_to_root_v3(data, index, u, dV); add_v3_v3(data->z[index], u); } @@ -1271,14 +1271,14 @@ void BPH_mass_spring_add_constraint_ndof1(Implicit_Data *data, int index, const void BPH_mass_spring_add_constraint_ndof2(Implicit_Data *data, int index, const float c1[3], const float dV[3]) { float m[3][3], p[3], u[3], cmat[3][3]; - + world_to_root_v3(data, index, p, c1); mul_fvectorT_fvector(cmat, p, p); sub_m3_m3m3(m, I, cmat); - + copy_m3_m3(data->S[index].m, m); // mul_m3_m3m3(data->S[index].m, data->S[index].m, m); - + world_to_root_v3(data, index, u, dV); add_v3_v3(data->z[index], u); } @@ -1289,7 +1289,7 @@ void BPH_mass_spring_clear_forces(Implicit_Data *data) zero_lfvector(data->F, numverts); init_bfmatrix(data->dFdX, ZERO); init_bfmatrix(data->dFdV, ZERO); - + data->num_blocks = 0; } @@ -1300,37 +1300,37 @@ void BPH_mass_spring_force_reference_frame(Implicit_Data *data, int index, const float f[3], dfdx[3][3], dfdv[3][3]; float euler[3], coriolis[3], centrifugal[3], rotvel[3]; float deuler[3][3], dcoriolis[3][3], dcentrifugal[3][3], drotvel[3][3]; - + world_to_root_v3(data, index, acc, acceleration); world_to_root_v3(data, index, w, omega); world_to_root_v3(data, index, dwdt, domega_dt); - + cross_v3_v3v3(euler, dwdt, data->X[index]); cross_v3_v3v3(coriolis, w, data->V[index]); mul_v3_fl(coriolis, 2.0f); cross_v3_v3v3(rotvel, w, data->X[index]); cross_v3_v3v3(centrifugal, w, rotvel); - + sub_v3_v3v3(f, acc, euler); sub_v3_v3(f, coriolis); sub_v3_v3(f, centrifugal); - + mul_v3_fl(f, mass); /* F = m * a */ - + cross_v3_identity(deuler, dwdt); cross_v3_identity(dcoriolis, w); mul_m3_fl(dcoriolis, 2.0f); cross_v3_identity(drotvel, w); cross_m3_v3m3(dcentrifugal, w, drotvel); - + add_m3_m3m3(dfdx, deuler, dcentrifugal); negate_m3(dfdx); mul_m3_fl(dfdx, mass); - + copy_m3_m3(dfdv, dcoriolis); negate_m3(dfdv); mul_m3_fl(dfdv, mass); - + add_v3_v3(data->F[index], f); add_m3_m3m3(data->dFdX[index].m, data->dFdX[index].m, dfdx); add_m3_m3m3(data->dFdV[index].m, data->dFdV[index].m, dfdv); @@ -1349,7 +1349,7 @@ void BPH_mass_spring_force_gravity(Implicit_Data *data, int index, float mass, c float f[3]; world_to_root_v3(data, index, f, g); mul_v3_fl(f, mass); - + add_v3_v3(data->F[index], f); } @@ -1358,10 +1358,10 @@ void BPH_mass_spring_force_drag(Implicit_Data *data, float drag) int i, numverts = data->M[0].vcount; for (i = 0; i < numverts; i++) { float tmp[3][3]; - + /* NB: uses root space velocity, no need to transform */ madd_v3_v3fl(data->F[i], data->V[i], -drag); - + copy_m3_m3(tmp, I); mul_m3_fl(tmp, -drag); add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, tmp); @@ -1374,7 +1374,7 @@ void BPH_mass_spring_force_extern(struct Implicit_Data *data, int i, const float world_to_root_v3(data, i, tf, f); world_to_root_m3(data, i, tdfdx, dfdx); world_to_root_m3(data, i, tdfdv, dfdv); - + add_v3_v3(data->F[i], tf); add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, tdfdx); add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, tdfdv); @@ -1383,10 +1383,10 @@ void BPH_mass_spring_force_extern(struct Implicit_Data *data, int i, const float static float calc_nor_area_tri(float nor[3], const float v1[3], const float v2[3], const float v3[3]) { float n1[3], n2[3]; - + sub_v3_v3v3(n1, v1, v2); sub_v3_v3v3(n2, v2, v3); - + cross_v3_v3v3(nor, n1, n2); return normalize_v3(nor); } @@ -1397,17 +1397,17 @@ void BPH_mass_spring_force_face_wind(Implicit_Data *data, int v1, int v2, int v3 const float effector_scale = 0.02f; float win[3], nor[3], area; float factor; - + /* calculate face normal and area */ area = calc_nor_area_tri(nor, data->X[v1], data->X[v2], data->X[v3]); factor = effector_scale * area / 3.0f; - + world_to_root_v3(data, v1, win, winvec[v1]); madd_v3_v3fl(data->F[v1], nor, factor * dot_v3v3(win, nor)); - + world_to_root_v3(data, v2, win, winvec[v2]); madd_v3_v3fl(data->F[v2], nor, factor * dot_v3v3(win, nor)); - + world_to_root_v3(data, v3, win, winvec[v3]); madd_v3_v3fl(data->F[v3], nor, factor * dot_v3v3(win, nor)); } @@ -1417,17 +1417,17 @@ static void edge_wind_vertex(const float dir[3], float length, float radius, con const float density = 0.01f; /* XXX arbitrary value, corresponds to effect of air density */ float cos_alpha, sin_alpha, cross_section; float windlen = len_v3(wind); - + if (windlen == 0.0f) { zero_v3(f); return; } - + /* angle of wind direction to edge */ cos_alpha = dot_v3v3(wind, dir) / windlen; sin_alpha = sqrtf(1.0f - cos_alpha * cos_alpha); cross_section = radius * ((float)M_PI * radius * sin_alpha + length * cos_alpha); - + mul_v3_v3fl(f, wind, density * cross_section); } @@ -1435,14 +1435,14 @@ void BPH_mass_spring_force_edge_wind(Implicit_Data *data, int v1, int v2, float { float win[3], dir[3], length; float f[3], dfdx[3][3], dfdv[3][3]; - + sub_v3_v3v3(dir, data->X[v1], data->X[v2]); length = normalize_v3(dir); - + world_to_root_v3(data, v1, win, winvec[v1]); edge_wind_vertex(dir, length, radius1, win, f, dfdx, dfdv); add_v3_v3(data->F[v1], f); - + world_to_root_v3(data, v2, win, winvec[v2]); edge_wind_vertex(dir, length, radius2, win, f, dfdx, dfdv); add_v3_v3(data->F[v2], f); @@ -1451,10 +1451,10 @@ void BPH_mass_spring_force_edge_wind(Implicit_Data *data, int v1, int v2, float void BPH_mass_spring_force_vertex_wind(Implicit_Data *data, int v, float UNUSED(radius), const float (*winvec)[3]) { const float density = 0.01f; /* XXX arbitrary value, corresponds to effect of air density */ - + float wind[3]; float f[3]; - + world_to_root_v3(data, v, wind, winvec[v]); mul_v3_v3fl(f, wind, density); add_v3_v3(data->F[v], f); @@ -1466,8 +1466,8 @@ BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, fl //return ( (I-outerprod(dir, dir))*Min(1.0f, rest/length) - I) * -k; outerproduct(to, dir, dir); sub_m3_m3m3(to, I, to); - - mul_m3_fl(to, (L/length)); + + mul_m3_fl(to, (L/length)); sub_m3_m3m3(to, to, I); mul_m3_fl(to, k); } @@ -1476,7 +1476,7 @@ BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, fl #if 0 BLI_INLINE void dfdx_damp(float to[3][3], const float dir[3], float length, const float vel[3], float rest, float damping) { - // inner spring damping vel is the relative velocity of the endpoints. + // inner spring damping vel is the relative velocity of the endpoints. // return (I-outerprod(dir, dir)) * (-damping * -(dot(dir, vel)/Max(length, rest))); mul_fvectorT_fvector(to, dir, dir); sub_fmatrix_fmatrix(to, I, to); @@ -1512,7 +1512,7 @@ BLI_INLINE float fbstar(float length, float L, float kb, float cb) { float tempfb_fl = kb * fb(length, L); float fbstar_fl = cb * (length - L); - + if (tempfb_fl < fbstar_fl) return fbstar_fl; else @@ -1539,7 +1539,7 @@ BLI_INLINE bool spring_length(Implicit_Data *data, int i, int j, float r_extent[ sub_v3_v3v3(r_extent, data->X[j], data->X[i]); sub_v3_v3v3(r_vel, data->V[j], data->V[i]); *r_length = len_v3(r_extent); - + if (*r_length > ALMOST_ZERO) { /* if (length>L) { @@ -1557,21 +1557,21 @@ BLI_INLINE bool spring_length(Implicit_Data *data, int i, int j, float r_extent[ else { zero_v3(r_dir); } - + return true; } BLI_INLINE void apply_spring(Implicit_Data *data, int i, int j, const float f[3], float dfdx[3][3], float dfdv[3][3]) { int block_ij = BPH_mass_spring_add_block(data, i, j); - + add_v3_v3(data->F[i], f); sub_v3_v3(data->F[j], f); - + add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfdx); add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfdx); sub_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfdx); - + add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, dfdv); add_m3_m3m3(data->dFdV[j].m, data->dFdV[j].m, dfdv); sub_m3_m3m3(data->dFdV[block_ij].m, data->dFdV[block_ij].m, dfdv); @@ -1581,7 +1581,7 @@ bool BPH_mass_spring_force_spring_linear(Implicit_Data *data, int i, int j, floa float stiffness, float damping, bool no_compress, float clamp_force) { float extent[3], length, dir[3], vel[3]; - + // calculate elonglation spring_length(data, i, j, extent, dir, &length, vel); @@ -1590,22 +1590,22 @@ bool BPH_mass_spring_force_spring_linear(Implicit_Data *data, int i, int j, floa Thus length > restlen makes cloth unconstrained at the start of simulation. */ if ((length >= restlen && length > 0) || no_compress) { float stretch_force, f[3], dfdx[3][3], dfdv[3][3]; - + stretch_force = stiffness * (length - restlen); if (clamp_force > 0.0f && stretch_force > clamp_force) { stretch_force = clamp_force; } mul_v3_v3fl(f, dir, stretch_force); - + // Ascher & Boxman, p.21: Damping only during elonglation // something wrong with it... madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir)); - + dfdx_spring(dfdx, dir, length, restlen, stiffness); dfdv_damp(dfdv, dir, damping); - + apply_spring(data, i, j, f, dfdx, dfdv); - + return true; } else { @@ -1617,23 +1617,23 @@ bool BPH_mass_spring_force_spring_linear(Implicit_Data *data, int i, int j, floa bool BPH_mass_spring_force_spring_bending(Implicit_Data *data, int i, int j, float restlen, float kb, float cb) { float extent[3], length, dir[3], vel[3]; - + // calculate elonglation spring_length(data, i, j, extent, dir, &length, vel); - + if (length < restlen) { float f[3], dfdx[3][3], dfdv[3][3]; - + mul_v3_v3fl(f, dir, fbstar(length, restlen, kb, cb)); - + outerproduct(dfdx, dir, dir); mul_m3_fl(dfdx, fbstar_jacobi(length, restlen, kb, cb)); - + /* XXX damping not supported */ zero_m3(dfdv); - + apply_spring(data, i, j, f, dfdx, dfdv); - + return true; } else { @@ -1650,10 +1650,10 @@ bool BPH_mass_spring_force_spring_bending(Implicit_Data *data, int i, int j, flo BLI_INLINE void spring_grad_dir(Implicit_Data *data, int i, int j, float edge[3], float dir[3], float grad_dir[3][3]) { float length; - + sub_v3_v3v3(edge, data->X[j], data->X[i]); length = normalize_v3_v3(dir, edge); - + if (length > ALMOST_ZERO) { outerproduct(grad_dir, dir, dir); sub_m3_m3m3(grad_dir, I, grad_dir); @@ -1676,39 +1676,39 @@ BLI_INLINE void spring_angbend_forces(Implicit_Data *data, int i, int j, int k, float f_bend[3], f_damp[3]; float fk[3]; float dist[3]; - + zero_v3(fk); - + sub_v3_v3v3(edge_ij, data->X[j], data->X[i]); if (q == i) sub_v3_v3(edge_ij, dx); if (q == j) add_v3_v3(edge_ij, dx); normalize_v3_v3(dir_ij, edge_ij); - + sub_v3_v3v3(edge_jk, data->X[k], data->X[j]); if (q == j) sub_v3_v3(edge_jk, dx); if (q == k) add_v3_v3(edge_jk, dx); normalize_v3_v3(dir_jk, edge_jk); - + sub_v3_v3v3(vel_ij, data->V[j], data->V[i]); if (q == i) sub_v3_v3(vel_ij, dv); if (q == j) add_v3_v3(vel_ij, dv); - + sub_v3_v3v3(vel_jk, data->V[k], data->V[j]); if (q == j) sub_v3_v3(vel_jk, dv); if (q == k) add_v3_v3(vel_jk, dv); - + /* bending force */ sub_v3_v3v3(dist, goal, edge_jk); mul_v3_v3fl(f_bend, dist, stiffness); - + add_v3_v3(fk, f_bend); - + /* damping force */ madd_v3_v3v3fl(vel_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk)); mul_v3_v3fl(f_damp, vel_ortho, damping); - + sub_v3_v3(fk, f_damp); - + copy_v3_v3(r_f, fk); } @@ -1722,24 +1722,24 @@ BLI_INLINE void spring_angbend_estimate_dfdx(Implicit_Data *data, int i, int j, float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3]; float f[3]; int a, b; - + zero_m3(dvec_null); unit_m3(dvec_pos); mul_m3_fl(dvec_pos, delta * 0.5f); copy_m3_m3(dvec_neg, dvec_pos); negate_m3(dvec_neg); - + /* XXX TODO offset targets to account for position dependency */ - + for (a = 0; a < 3; ++a) { spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_pos[a], dvec_null[a], f); copy_v3_v3(dfdx[a], f); - + spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_neg[a], dvec_null[a], f); sub_v3_v3(dfdx[a], f); - + for (b = 0; b < 3; ++b) { dfdx[a][b] /= delta; } @@ -1756,24 +1756,24 @@ BLI_INLINE void spring_angbend_estimate_dfdv(Implicit_Data *data, int i, int j, float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3]; float f[3]; int a, b; - + zero_m3(dvec_null); unit_m3(dvec_pos); mul_m3_fl(dvec_pos, delta * 0.5f); copy_m3_m3(dvec_neg, dvec_pos); negate_m3(dvec_neg); - + /* XXX TODO offset targets to account for position dependency */ - + for (a = 0; a < 3; ++a) { spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_null[a], dvec_pos[a], f); copy_v3_v3(dfdv[a], f); - + spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_null[a], dvec_neg[a], f); sub_v3_v3(dfdv[a], f); - + for (b = 0; b < 3; ++b) { dfdv[a][b] /= delta; } @@ -1790,45 +1790,45 @@ bool BPH_mass_spring_force_spring_bending_angular(Implicit_Data *data, int i, in float fj[3], fk[3]; float dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3]; float dfj_dvi[3][3], dfj_dvj[3][3], dfk_dvi[3][3], dfk_dvj[3][3], dfk_dvk[3][3]; - + const float vecnull[3] = {0.0f, 0.0f, 0.0f}; - + int block_ij = BPH_mass_spring_add_block(data, i, j); int block_jk = BPH_mass_spring_add_block(data, j, k); int block_ik = BPH_mass_spring_add_block(data, i, k); - + world_to_root_v3(data, j, goal, target); - + spring_angbend_forces(data, i, j, k, goal, stiffness, damping, k, vecnull, vecnull, fk); negate_v3_v3(fj, fk); /* counterforce */ - + spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, i, dfk_dxi); spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, j, dfk_dxj); spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, k, dfk_dxk); copy_m3_m3(dfj_dxi, dfk_dxi); negate_m3(dfj_dxi); copy_m3_m3(dfj_dxj, dfk_dxj); negate_m3(dfj_dxj); - + spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, i, dfk_dvi); spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, j, dfk_dvj); spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, k, dfk_dvk); copy_m3_m3(dfj_dvi, dfk_dvi); negate_m3(dfj_dvi); copy_m3_m3(dfj_dvj, dfk_dvj); negate_m3(dfj_dvj); - + /* add forces and jacobians to the solver data */ - + add_v3_v3(data->F[j], fj); add_v3_v3(data->F[k], fk); - + add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfj_dxj); add_m3_m3m3(data->dFdX[k].m, data->dFdX[k].m, dfk_dxk); - + add_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfj_dxi); add_m3_m3m3(data->dFdX[block_jk].m, data->dFdX[block_jk].m, dfk_dxj); add_m3_m3m3(data->dFdX[block_ik].m, data->dFdX[block_ik].m, dfk_dxi); - + add_m3_m3m3(data->dFdV[j].m, data->dFdV[j].m, dfj_dvj); add_m3_m3m3(data->dFdV[k].m, data->dFdV[k].m, dfk_dvk); - + add_m3_m3m3(data->dFdV[block_ij].m, data->dFdV[block_ij].m, dfj_dvi); add_m3_m3m3(data->dFdV[block_jk].m, data->dFdV[block_jk].m, dfk_dvj); add_m3_m3m3(data->dFdV[block_ik].m, data->dFdV[block_ik].m, dfk_dvi); @@ -1847,10 +1847,10 @@ bool BPH_mass_spring_force_spring_bending_angular(Implicit_Data *data, int i, in float fi[3], fj[3], fk[3]; float dfi_dxi[3][3], dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3]; float dfdvi[3][3]; - + // TESTING damping = 0.0f; - + zero_v3(fi); zero_v3(fj); zero_v3(fk); @@ -1859,68 +1859,68 @@ bool BPH_mass_spring_force_spring_bending_angular(Implicit_Data *data, int i, in zero_m3(dfk_dxi); zero_m3(dfk_dxj); zero_m3(dfk_dxk); - + /* jacobian of direction vectors */ spring_grad_dir(data, i, j, edge_ij, dir_ij, grad_dir_ij); spring_grad_dir(data, j, k, edge_jk, dir_jk, grad_dir_jk); - + sub_v3_v3v3(vel_jk, data->V[k], data->V[j]); - + /* bending force */ mul_v3_v3fl(target, dir_ij, restlen); sub_v3_v3v3(dist, target, edge_jk); mul_v3_v3fl(fk, dist, stiffness); - + /* damping force */ madd_v3_v3v3fl(vel_jk_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk)); madd_v3_v3fl(fk, vel_jk_ortho, damping); - + /* XXX this only holds true as long as we assume straight rest shape! * eventually will become a bit more involved since the opposite segment * gets its own target, under condition of having equal torque on both sides. */ copy_v3_v3(fi, fk); - + /* counterforce on the middle point */ sub_v3_v3(fj, fi); sub_v3_v3(fj, fk); - + /* === derivatives === */ - + madd_m3_m3fl(dfk_dxi, grad_dir_ij, stiffness * restlen); - + madd_m3_m3fl(dfk_dxj, grad_dir_ij, -stiffness * restlen); madd_m3_m3fl(dfk_dxj, I, stiffness); - + madd_m3_m3fl(dfk_dxk, I, -stiffness); - + copy_m3_m3(dfi_dxi, dfk_dxk); negate_m3(dfi_dxi); - + /* dfj_dfi == dfi_dfj due to symmetry, * dfi_dfj == dfk_dfj due to fi == fk * XXX see comment above on future bent rest shapes */ copy_m3_m3(dfj_dxi, dfk_dxj); - + /* dfj_dxj == -(dfi_dxj + dfk_dxj) due to fj == -(fi + fk) */ sub_m3_m3m3(dfj_dxj, dfj_dxj, dfj_dxi); sub_m3_m3m3(dfj_dxj, dfj_dxj, dfk_dxj); - + /* add forces and jacobians to the solver data */ add_v3_v3(data->F[i], fi); add_v3_v3(data->F[j], fj); add_v3_v3(data->F[k], fk); - + add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfi_dxi); add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfj_dxj); add_m3_m3m3(data->dFdX[k].m, data->dFdX[k].m, dfk_dxk); - + add_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfj_dxi); add_m3_m3m3(data->dFdX[block_jk].m, data->dFdX[block_jk].m, dfk_dxj); add_m3_m3m3(data->dFdX[block_ik].m, data->dFdX[block_ik].m, dfk_dxi); #endif - + return true; } @@ -1929,29 +1929,29 @@ bool BPH_mass_spring_force_spring_goal(Implicit_Data *data, int i, const float g { float root_goal_x[3], root_goal_v[3], extent[3], length, dir[3], vel[3]; float f[3], dfdx[3][3], dfdv[3][3]; - + /* goal is in world space */ world_to_root_v3(data, i, root_goal_x, goal_x); world_to_root_v3(data, i, root_goal_v, goal_v); - + sub_v3_v3v3(extent, root_goal_x, data->X[i]); sub_v3_v3v3(vel, root_goal_v, data->V[i]); length = normalize_v3_v3(dir, extent); - + if (length > ALMOST_ZERO) { mul_v3_v3fl(f, dir, stiffness * length); - + // Ascher & Boxman, p.21: Damping only during elonglation // something wrong with it... madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir)); - + dfdx_spring(dfdx, dir, length, 0.0f, stiffness); dfdv_damp(dfdv, dir, damping); - + add_v3_v3(data->F[i], f); add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfdx); add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, dfdv); - + return true; } else { diff --git a/source/blender/physics/intern/implicit_eigen.cpp b/source/blender/physics/intern/implicit_eigen.cpp index d56525f2e93..eaac63893a6 100644 --- a/source/blender/physics/intern/implicit_eigen.cpp +++ b/source/blender/physics/intern/implicit_eigen.cpp @@ -99,24 +99,24 @@ static float I[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; class fVector : public Eigen::Vector3f { public: typedef float *ctype; - + fVector() { } - + fVector(const ctype &v) { for (int k = 0; k < 3; ++k) coeffRef(k) = v[k]; } - + fVector& operator = (const ctype &v) { for (int k = 0; k < 3; ++k) coeffRef(k) = v[k]; return *this; } - + operator ctype() { return data(); @@ -129,18 +129,18 @@ public: class fMatrix : public Eigen::Matrix3f { public: typedef float (*ctype)[3]; - + fMatrix() { } - + fMatrix(const ctype &v) { for (int k = 0; k < 3; ++k) for (int l = 0; l < 3; ++l) coeffRef(l, k) = v[k][l]; } - + fMatrix& operator = (const ctype &v) { for (int k = 0; k < 3; ++k) @@ -148,7 +148,7 @@ public: coeffRef(l, k) = v[k][l]; return *this; } - + operator ctype() { return (ctype)data(); @@ -161,23 +161,23 @@ public: class lVector : public Eigen::VectorXf { public: typedef Eigen::VectorXf base_t; - + lVector() { } - + template <typename T> lVector& operator = (T rhs) { base_t::operator=(rhs); return *this; } - + float* v3(int vertex) { return &coeffRef(3 * vertex); } - + const float* v3(int vertex) const { return &coeffRef(3 * vertex); @@ -198,18 +198,18 @@ struct lMatrixCtor { lMatrixCtor() { } - + void reset() { m_trips.clear(); } - + void reserve(int numverts) { /* reserve for diagonal entries */ m_trips.reserve(numverts * 9); } - + void add(int i, int j, const fMatrix &m) { i *= 3; @@ -218,7 +218,7 @@ struct lMatrixCtor { for (int l = 0; l < 3; ++l) m_trips.push_back(Triplet(i + k, j + l, m.coeff(l, k))); } - + void sub(int i, int j, const fMatrix &m) { i *= 3; @@ -227,13 +227,13 @@ struct lMatrixCtor { for (int l = 0; l < 3; ++l) m_trips.push_back(Triplet(i + k, j + l, -m.coeff(l, k))); } - + inline void construct(lMatrix &m) { m.setFromTriplets(m_trips.begin(), m_trips.end()); m_trips.clear(); } - + private: TripletList m_trips; }; @@ -253,7 +253,7 @@ static void print_lvector(const lVector &v) for (int i = 0; i < v.rows(); ++i) { if (i > 0 && i % 3 == 0) printf("\n"); - + printf("%f,\n", v[i]); } } @@ -263,11 +263,11 @@ static void print_lmatrix(const lMatrix &m) for (int j = 0; j < m.rows(); ++j) { if (j > 0 && j % 3 == 0) printf("\n"); - + for (int i = 0; i < m.cols(); ++i) { if (i > 0 && i % 3 == 0) printf(" "); - + implicit_print_matrix_elem(m.coeff(j, i)); } printf("\n"); @@ -383,63 +383,63 @@ BLI_INLINE void madd_m3_m3m3fl(float r[3][3], float a[3][3], float b[3][3], floa struct Implicit_Data { typedef std::vector<fMatrix> fMatrixVector; - + Implicit_Data(int numverts) { resize(numverts); } - + void resize(int numverts) { this->numverts = numverts; int tot = 3 * numverts; - + M.resize(tot, tot); F.resize(tot); dFdX.resize(tot, tot); dFdV.resize(tot, tot); - + tfm.resize(numverts, I); - + X.resize(tot); Xnew.resize(tot); V.resize(tot); Vnew.resize(tot); - + A.resize(tot, tot); B.resize(tot); - + dV.resize(tot); z.resize(tot); S.resize(tot, tot); - + iM.reserve(numverts); idFdX.reserve(numverts); idFdV.reserve(numverts); iS.reserve(numverts); } - + int numverts; - + /* inputs */ lMatrix M; /* masses */ lVector F; /* forces */ lMatrix dFdX, dFdV; /* force jacobians */ - + fMatrixVector tfm; /* local coordinate transform */ - + /* motion state data */ lVector X, Xnew; /* positions */ lVector V, Vnew; /* velocities */ - + /* internal solver data */ lVector B; /* B for A*dV = B */ lMatrix A; /* A for A*dV = B */ - + lVector dV; /* velocity change (solution of A*dV = B) */ lVector z; /* target velocity in constrained directions */ lMatrix S; /* filtering matrix for constraints */ - + /* temporary constructors */ lMatrixCtor iM; /* masses */ lMatrixCtor idFdX, idFdV; /* force jacobians */ @@ -502,25 +502,25 @@ bool BPH_mass_spring_solve_velocities(Implicit_Data *data, float dt, ImplicitSol #ifdef USE_EIGEN_CONSTRAINED_CG typedef ConstraintConjGrad solver_t; #endif - + data->iM.construct(data->M); data->idFdX.construct(data->dFdX); data->idFdV.construct(data->dFdV); data->iS.construct(data->S); - + solver_t cg; cg.setMaxIterations(100); cg.setTolerance(0.01f); - + #ifdef USE_EIGEN_CONSTRAINED_CG cg.filter() = data->S; #endif - + data->A = data->M - dt * data->dFdV - dt*dt * data->dFdX; cg.compute(data->A); - + data->B = dt * data->F + dt*dt * data->dFdX * data->V; - + #ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT printf("==== A ====\n"); print_lmatrix(id->A); @@ -531,22 +531,22 @@ bool BPH_mass_spring_solve_velocities(Implicit_Data *data, float dt, ImplicitSol printf("==== S ====\n"); print_lmatrix(id->S); #endif - + #ifdef USE_EIGEN_CORE data->dV = cg.solve(data->B); #endif #ifdef USE_EIGEN_CONSTRAINED_CG data->dV = cg.solveWithGuess(data->B, data->z); #endif - + #ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT printf("==== dV ====\n"); print_lvector(id->dV); printf("========\n"); #endif - + data->Vnew = data->V + data->dV; - + switch (cg.info()) { case Eigen::Success: result->status = BPH_SOLVER_SUCCESS; break; case Eigen::NoConvergence: result->status = BPH_SOLVER_NO_CONVERGENCE; break; @@ -556,7 +556,7 @@ bool BPH_mass_spring_solve_velocities(Implicit_Data *data, float dt, ImplicitSol result->iterations = cg.iterations(); result->error = cg.error(); - + return cg.info() == Eigen::Success; } @@ -641,26 +641,26 @@ void BPH_mass_spring_clear_constraints(Implicit_Data *data) void BPH_mass_spring_add_constraint_ndof0(Implicit_Data *data, int index, const float dV[3]) { data->iS.sub(index, index, I); - + world_to_root_v3(data, index, data->z.v3(index), dV); } void BPH_mass_spring_add_constraint_ndof1(Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3]) { float m[3][3], p[3], q[3], u[3], cmat[3][3]; - + world_to_root_v3(data, index, p, c1); outerproduct(cmat, p, p); copy_m3_m3(m, cmat); - + world_to_root_v3(data, index, q, c2); outerproduct(cmat, q, q); add_m3_m3m3(m, m, cmat); - + /* XXX not sure but multiplication should work here */ data->iS.sub(index, index, m); // mul_m3_m3m3(data->S[index].m, data->S[index].m, m); - + world_to_root_v3(data, index, u, dV); add_v3_v3(data->z.v3(index), u); } @@ -668,14 +668,14 @@ void BPH_mass_spring_add_constraint_ndof1(Implicit_Data *data, int index, const void BPH_mass_spring_add_constraint_ndof2(Implicit_Data *data, int index, const float c1[3], const float dV[3]) { float m[3][3], p[3], u[3], cmat[3][3]; - + world_to_root_v3(data, index, p, c1); outerproduct(cmat, p, p); copy_m3_m3(m, cmat); - + data->iS.sub(index, index, m); // mul_m3_m3m3(data->S[index].m, data->S[index].m, m); - + world_to_root_v3(data, index, u, dV); add_v3_v3(data->z.v3(index), u); } @@ -694,37 +694,37 @@ void BPH_mass_spring_force_reference_frame(Implicit_Data *data, int index, const float f[3], dfdx[3][3], dfdv[3][3]; float euler[3], coriolis[3], centrifugal[3], rotvel[3]; float deuler[3][3], dcoriolis[3][3], dcentrifugal[3][3], drotvel[3][3]; - + world_to_root_v3(data, index, acc, acceleration); world_to_root_v3(data, index, w, omega); world_to_root_v3(data, index, dwdt, domega_dt); - + cross_v3_v3v3(euler, dwdt, data->X.v3(index)); cross_v3_v3v3(coriolis, w, data->V.v3(index)); mul_v3_fl(coriolis, 2.0f); cross_v3_v3v3(rotvel, w, data->X.v3(index)); cross_v3_v3v3(centrifugal, w, rotvel); - + sub_v3_v3v3(f, acc, euler); sub_v3_v3(f, coriolis); sub_v3_v3(f, centrifugal); - + mul_v3_fl(f, mass); /* F = m * a */ - + cross_v3_identity(deuler, dwdt); cross_v3_identity(dcoriolis, w); mul_m3_fl(dcoriolis, 2.0f); cross_v3_identity(drotvel, w); cross_m3_v3m3(dcentrifugal, w, drotvel); - + add_m3_m3m3(dfdx, deuler, dcentrifugal); negate_m3(dfdx); mul_m3_fl(dfdx, mass); - + copy_m3_m3(dfdv, dcoriolis); negate_m3(dfdv); mul_m3_fl(dfdv, mass); - + add_v3_v3(data->F.v3(index), f); data->idFdX.add(index, index, dfdx); data->idFdV.add(index, index, dfdv); @@ -743,7 +743,7 @@ void BPH_mass_spring_force_gravity(Implicit_Data *data, int index, float mass, c float f[3]; world_to_root_v3(data, index, f, g); mul_v3_fl(f, mass); - + add_v3_v3(data->F.v3(index), f); } @@ -752,10 +752,10 @@ void BPH_mass_spring_force_drag(Implicit_Data *data, float drag) int numverts = data->numverts; for (int i = 0; i < numverts; i++) { float tmp[3][3]; - + /* NB: uses root space velocity, no need to transform */ madd_v3_v3fl(data->F.v3(i), data->V.v3(i), -drag); - + copy_m3_m3(tmp, I); mul_m3_fl(tmp, -drag); data->idFdV.add(i, i, tmp); @@ -768,7 +768,7 @@ void BPH_mass_spring_force_extern(struct Implicit_Data *data, int i, const float world_to_root_v3(data, i, tf, f); world_to_root_m3(data, i, tdfdx, dfdx); world_to_root_m3(data, i, tdfdv, dfdv); - + add_v3_v3(data->F.v3(i), tf); data->idFdX.add(i, i, tdfdx); data->idFdV.add(i, i, tdfdv); @@ -777,10 +777,10 @@ void BPH_mass_spring_force_extern(struct Implicit_Data *data, int i, const float static float calc_nor_area_tri(float nor[3], const float v1[3], const float v2[3], const float v3[3]) { float n1[3], n2[3]; - + sub_v3_v3v3(n1, v1, v2); sub_v3_v3v3(n2, v2, v3); - + cross_v3_v3v3(nor, n1, n2); return normalize_v3(nor); } @@ -791,17 +791,17 @@ void BPH_mass_spring_force_face_wind(Implicit_Data *data, int v1, int v2, int v3 const float effector_scale = 0.02f; float win[3], nor[3], area; float factor; - + // calculate face normal and area area = calc_nor_area_tri(nor, data->X.v3(v1), data->X.v3(v2), data->X.v3(v3)); factor = effector_scale * area / 3.0f; - + world_to_root_v3(data, v1, win, winvec[v1]); madd_v3_v3fl(data->F.v3(v1), nor, factor * dot_v3v3(win, nor)); - + world_to_root_v3(data, v2, win, winvec[v2]); madd_v3_v3fl(data->F.v3(v2), nor, factor * dot_v3v3(win, nor)); - + world_to_root_v3(data, v3, win, winvec[v3]); madd_v3_v3fl(data->F.v3(v3), nor, factor * dot_v3v3(win, nor)); } @@ -810,14 +810,14 @@ void BPH_mass_spring_force_edge_wind(Implicit_Data *data, int v1, int v2, const { const float effector_scale = 0.01; float win[3], dir[3], nor[3], length; - + sub_v3_v3v3(dir, data->X.v3(v1), data->X.v3(v2)); length = normalize_v3(dir); - + world_to_root_v3(data, v1, win, winvec[v1]); madd_v3_v3v3fl(nor, win, dir, -dot_v3v3(win, dir)); madd_v3_v3fl(data->F.v3(v1), nor, effector_scale * length); - + world_to_root_v3(data, v2, win, winvec[v2]); madd_v3_v3v3fl(nor, win, dir, -dot_v3v3(win, dir)); madd_v3_v3fl(data->F.v3(v2), nor, effector_scale * length); @@ -829,8 +829,8 @@ BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, fl //return ( (I-outerprod(dir, dir))*Min(1.0f, rest/length) - I) * -k; outerproduct(to, dir, dir); sub_m3_m3m3(to, I, to); - - mul_m3_fl(to, (L/length)); + + mul_m3_fl(to, (L/length)); sub_m3_m3m3(to, to, I); mul_m3_fl(to, k); } @@ -839,7 +839,7 @@ BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, fl #if 0 BLI_INLINE void dfdx_damp(float to[3][3], const float dir[3], float length, const float vel[3], float rest, float damping) { - // inner spring damping vel is the relative velocity of the endpoints. + // inner spring damping vel is the relative velocity of the endpoints. // return (I-outerprod(dir, dir)) * (-damping * -(dot(dir, vel)/Max(length, rest))); mul_fvectorT_fvector(to, dir, dir); sub_fmatrix_fmatrix(to, I, to); @@ -871,7 +871,7 @@ BLI_INLINE float fbstar(float length, float L, float kb, float cb) { float tempfb_fl = kb * fb(length, L); float fbstar_fl = cb * (length - L); - + if (tempfb_fl < fbstar_fl) return fbstar_fl; else @@ -898,7 +898,7 @@ BLI_INLINE bool spring_length(Implicit_Data *data, int i, int j, float r_extent[ sub_v3_v3v3(r_extent, data->X.v3(j), data->X.v3(i)); sub_v3_v3v3(r_vel, data->V.v3(j), data->V.v3(i)); *r_length = len_v3(r_extent); - + if (*r_length > ALMOST_ZERO) { /* if (length>L) { @@ -916,7 +916,7 @@ BLI_INLINE bool spring_length(Implicit_Data *data, int i, int j, float r_extent[ else { zero_v3(r_dir); } - + return true; } @@ -924,12 +924,12 @@ BLI_INLINE void apply_spring(Implicit_Data *data, int i, int j, const float f[3] { add_v3_v3(data->F.v3(i), f); sub_v3_v3(data->F.v3(j), f); - + data->idFdX.add(i, i, dfdx); data->idFdX.add(j, j, dfdx); data->idFdX.sub(i, j, dfdx); data->idFdX.sub(j, i, dfdx); - + data->idFdV.add(i, i, dfdv); data->idFdV.add(j, j, dfdv); data->idFdV.sub(i, j, dfdv); @@ -941,39 +941,39 @@ bool BPH_mass_spring_force_spring_linear(Implicit_Data *data, int i, int j, floa float r_f[3], float r_dfdx[3][3], float r_dfdv[3][3]) { float extent[3], length, dir[3], vel[3]; - + // calculate elonglation spring_length(data, i, j, extent, dir, &length, vel); - + if (length > restlen || no_compress) { float stretch_force, f[3], dfdx[3][3], dfdv[3][3]; - + stretch_force = stiffness * (length - restlen); if (clamp_force > 0.0f && stretch_force > clamp_force) { stretch_force = clamp_force; } mul_v3_v3fl(f, dir, stretch_force); - + // Ascher & Boxman, p.21: Damping only during elonglation // something wrong with it... madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir)); - + dfdx_spring(dfdx, dir, length, restlen, stiffness); dfdv_damp(dfdv, dir, damping); - + apply_spring(data, i, j, f, dfdx, dfdv); - + if (r_f) copy_v3_v3(r_f, f); if (r_dfdx) copy_m3_m3(r_dfdx, dfdx); if (r_dfdv) copy_m3_m3(r_dfdv, dfdv); - + return true; } else { if (r_f) zero_v3(r_f); if (r_dfdx) zero_m3(r_dfdx); if (r_dfdv) zero_m3(r_dfdv); - + return false; } } @@ -984,34 +984,34 @@ bool BPH_mass_spring_force_spring_bending(Implicit_Data *data, int i, int j, flo float r_f[3], float r_dfdx[3][3], float r_dfdv[3][3]) { float extent[3], length, dir[3], vel[3]; - + // calculate elonglation spring_length(data, i, j, extent, dir, &length, vel); - + if (length < restlen) { float f[3], dfdx[3][3], dfdv[3][3]; - + mul_v3_v3fl(f, dir, fbstar(length, restlen, kb, cb)); - + outerproduct(dfdx, dir, dir); mul_m3_fl(dfdx, fbstar_jacobi(length, restlen, kb, cb)); - + /* XXX damping not supported */ zero_m3(dfdv); - + apply_spring(data, i, j, f, dfdx, dfdv); - + if (r_f) copy_v3_v3(r_f, f); if (r_dfdx) copy_m3_m3(r_dfdx, dfdx); if (r_dfdv) copy_m3_m3(r_dfdv, dfdv); - + return true; } else { if (r_f) zero_v3(r_f); if (r_dfdx) zero_m3(r_dfdx); if (r_dfdv) zero_m3(r_dfdv); - + return false; } } @@ -1025,10 +1025,10 @@ bool BPH_mass_spring_force_spring_bending(Implicit_Data *data, int i, int j, flo BLI_INLINE void spring_grad_dir(Implicit_Data *data, int i, int j, float edge[3], float dir[3], float grad_dir[3][3]) { float length; - + sub_v3_v3v3(edge, data->X.v3(j), data->X.v3(i)); length = normalize_v3_v3(dir, edge); - + if (length > ALMOST_ZERO) { outerproduct(grad_dir, dir, dir); sub_m3_m3m3(grad_dir, I, grad_dir); @@ -1051,39 +1051,39 @@ BLI_INLINE void spring_angbend_forces(Implicit_Data *data, int i, int j, int k, float f_bend[3], f_damp[3]; float fk[3]; float dist[3]; - + zero_v3(fk); - + sub_v3_v3v3(edge_ij, data->X.v3(j), data->X.v3(i)); if (q == i) sub_v3_v3(edge_ij, dx); if (q == j) add_v3_v3(edge_ij, dx); normalize_v3_v3(dir_ij, edge_ij); - + sub_v3_v3v3(edge_jk, data->X.v3(k), data->X.v3(j)); if (q == j) sub_v3_v3(edge_jk, dx); if (q == k) add_v3_v3(edge_jk, dx); normalize_v3_v3(dir_jk, edge_jk); - + sub_v3_v3v3(vel_ij, data->V.v3(j), data->V.v3(i)); if (q == i) sub_v3_v3(vel_ij, dv); if (q == j) add_v3_v3(vel_ij, dv); - + sub_v3_v3v3(vel_jk, data->V.v3(k), data->V.v3(j)); if (q == j) sub_v3_v3(vel_jk, dv); if (q == k) add_v3_v3(vel_jk, dv); - + /* bending force */ sub_v3_v3v3(dist, goal, edge_jk); mul_v3_v3fl(f_bend, dist, stiffness); - + add_v3_v3(fk, f_bend); - + /* damping force */ madd_v3_v3v3fl(vel_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk)); mul_v3_v3fl(f_damp, vel_ortho, damping); - + sub_v3_v3(fk, f_damp); - + copy_v3_v3(r_f, fk); } @@ -1097,24 +1097,24 @@ BLI_INLINE void spring_angbend_estimate_dfdx(Implicit_Data *data, int i, int j, float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3]; float f[3]; int a, b; - + zero_m3(dvec_null); unit_m3(dvec_pos); mul_m3_fl(dvec_pos, delta * 0.5f); copy_m3_m3(dvec_neg, dvec_pos); negate_m3(dvec_neg); - + /* XXX TODO offset targets to account for position dependency */ - + for (a = 0; a < 3; ++a) { spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_pos[a], dvec_null[a], f); copy_v3_v3(dfdx[a], f); - + spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_neg[a], dvec_null[a], f); sub_v3_v3(dfdx[a], f); - + for (b = 0; b < 3; ++b) { dfdx[a][b] /= delta; } @@ -1131,24 +1131,24 @@ BLI_INLINE void spring_angbend_estimate_dfdv(Implicit_Data *data, int i, int j, float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3]; float f[3]; int a, b; - + zero_m3(dvec_null); unit_m3(dvec_pos); mul_m3_fl(dvec_pos, delta * 0.5f); copy_m3_m3(dvec_neg, dvec_pos); negate_m3(dvec_neg); - + /* XXX TODO offset targets to account for position dependency */ - + for (a = 0; a < 3; ++a) { spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_null[a], dvec_pos[a], f); copy_v3_v3(dfdv[a], f); - + spring_angbend_forces(data, i, j, k, goal, stiffness, damping, q, dvec_null[a], dvec_neg[a], f); sub_v3_v3(dfdv[a], f); - + for (b = 0; b < 3; ++b) { dfdv[a][b] /= delta; } @@ -1165,44 +1165,44 @@ bool BPH_mass_spring_force_spring_bending_angular(Implicit_Data *data, int i, in float fj[3], fk[3]; float dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3]; float dfj_dvi[3][3], dfj_dvj[3][3], dfk_dvi[3][3], dfk_dvj[3][3], dfk_dvk[3][3]; - + const float vecnull[3] = {0.0f, 0.0f, 0.0f}; - + world_to_root_v3(data, j, goal, target); - + spring_angbend_forces(data, i, j, k, goal, stiffness, damping, k, vecnull, vecnull, fk); negate_v3_v3(fj, fk); /* counterforce */ - + spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, i, dfk_dxi); spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, j, dfk_dxj); spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, k, dfk_dxk); copy_m3_m3(dfj_dxi, dfk_dxi); negate_m3(dfj_dxi); copy_m3_m3(dfj_dxj, dfk_dxj); negate_m3(dfj_dxj); - + spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, i, dfk_dvi); spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, j, dfk_dvj); spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, k, dfk_dvk); copy_m3_m3(dfj_dvi, dfk_dvi); negate_m3(dfj_dvi); copy_m3_m3(dfj_dvj, dfk_dvj); negate_m3(dfj_dvj); - + /* add forces and jacobians to the solver data */ - + add_v3_v3(data->F.v3(j), fj); add_v3_v3(data->F.v3(k), fk); - + data->idFdX.add(j, j, dfj_dxj); data->idFdX.add(k, k, dfk_dxk); - + data->idFdX.add(i, j, dfj_dxi); data->idFdX.add(j, i, dfj_dxi); data->idFdX.add(j, k, dfk_dxj); data->idFdX.add(k, j, dfk_dxj); data->idFdX.add(i, k, dfk_dxi); data->idFdX.add(k, i, dfk_dxi); - + data->idFdV.add(j, j, dfj_dvj); data->idFdV.add(k, k, dfk_dvk); - + data->idFdV.add(i, j, dfj_dvi); data->idFdV.add(j, i, dfj_dvi); data->idFdV.add(j, k, dfk_dvj); @@ -1223,10 +1223,10 @@ bool BPH_mass_spring_force_spring_bending_angular(Implicit_Data *data, int i, in float fi[3], fj[3], fk[3]; float dfi_dxi[3][3], dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3]; float dfdvi[3][3]; - + // TESTING damping = 0.0f; - + zero_v3(fi); zero_v3(fj); zero_v3(fk); @@ -1235,68 +1235,68 @@ bool BPH_mass_spring_force_spring_bending_angular(Implicit_Data *data, int i, in zero_m3(dfk_dxi); zero_m3(dfk_dxj); zero_m3(dfk_dxk); - + /* jacobian of direction vectors */ spring_grad_dir(data, i, j, edge_ij, dir_ij, grad_dir_ij); spring_grad_dir(data, j, k, edge_jk, dir_jk, grad_dir_jk); - + sub_v3_v3v3(vel_jk, data->V[k], data->V[j]); - + /* bending force */ mul_v3_v3fl(target, dir_ij, restlen); sub_v3_v3v3(dist, target, edge_jk); mul_v3_v3fl(fk, dist, stiffness); - + /* damping force */ madd_v3_v3v3fl(vel_jk_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk)); madd_v3_v3fl(fk, vel_jk_ortho, damping); - + /* XXX this only holds true as long as we assume straight rest shape! * eventually will become a bit more involved since the opposite segment * gets its own target, under condition of having equal torque on both sides. */ copy_v3_v3(fi, fk); - + /* counterforce on the middle point */ sub_v3_v3(fj, fi); sub_v3_v3(fj, fk); - + /* === derivatives === */ - + madd_m3_m3fl(dfk_dxi, grad_dir_ij, stiffness * restlen); - + madd_m3_m3fl(dfk_dxj, grad_dir_ij, -stiffness * restlen); madd_m3_m3fl(dfk_dxj, I, stiffness); - + madd_m3_m3fl(dfk_dxk, I, -stiffness); - + copy_m3_m3(dfi_dxi, dfk_dxk); negate_m3(dfi_dxi); - + /* dfj_dfi == dfi_dfj due to symmetry, * dfi_dfj == dfk_dfj due to fi == fk * XXX see comment above on future bent rest shapes */ copy_m3_m3(dfj_dxi, dfk_dxj); - + /* dfj_dxj == -(dfi_dxj + dfk_dxj) due to fj == -(fi + fk) */ sub_m3_m3m3(dfj_dxj, dfj_dxj, dfj_dxi); sub_m3_m3m3(dfj_dxj, dfj_dxj, dfk_dxj); - + /* add forces and jacobians to the solver data */ add_v3_v3(data->F[i], fi); add_v3_v3(data->F[j], fj); add_v3_v3(data->F[k], fk); - + add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfi_dxi); add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfj_dxj); add_m3_m3m3(data->dFdX[k].m, data->dFdX[k].m, dfk_dxk); - + add_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfj_dxi); add_m3_m3m3(data->dFdX[block_jk].m, data->dFdX[block_jk].m, dfk_dxj); add_m3_m3m3(data->dFdX[block_ik].m, data->dFdX[block_ik].m, dfk_dxi); #endif - + return true; } @@ -1306,40 +1306,40 @@ bool BPH_mass_spring_force_spring_goal(Implicit_Data *data, int i, const float g { float root_goal_x[3], root_goal_v[3], extent[3], length, dir[3], vel[3]; float f[3], dfdx[3][3], dfdv[3][3]; - + /* goal is in world space */ world_to_root_v3(data, i, root_goal_x, goal_x); world_to_root_v3(data, i, root_goal_v, goal_v); - + sub_v3_v3v3(extent, root_goal_x, data->X.v3(i)); sub_v3_v3v3(vel, root_goal_v, data->V.v3(i)); length = normalize_v3_v3(dir, extent); - + if (length > ALMOST_ZERO) { mul_v3_v3fl(f, dir, stiffness * length); - + // Ascher & Boxman, p.21: Damping only during elonglation // something wrong with it... madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir)); - + dfdx_spring(dfdx, dir, length, 0.0f, stiffness); dfdv_damp(dfdv, dir, damping); - + add_v3_v3(data->F.v3(i), f); data->idFdX.add(i, i, dfdx); data->idFdV.add(i, i, dfdv); - + if (r_f) copy_v3_v3(r_f, f); if (r_dfdx) copy_m3_m3(r_dfdx, dfdx); if (r_dfdv) copy_m3_m3(r_dfdv, dfdv); - + return true; } else { if (r_f) zero_v3(r_f); if (r_dfdx) zero_m3(r_dfdx); if (r_dfdv) zero_m3(r_dfdv); - + return false; } } |