archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity

Merge branch 'master' into blender2.8

Browse Source
This commit is contained in:
Campbell Barton
2018-06-08 08:10:35 +02:00
parent d352a0adc5 a25c11fd8d
commit 908b6960c0
191 changed files with 36025 additions and 2057 deletions
Download Patch File Download Diff File Expand all files Collapse all files

334
source/blender/physics/intern/BPH_mass_spring.cpp
View File

@@ -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,23 +1071,23 @@ 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;
}

34
source/blender/physics/intern/ConstrainedConjugateGradient.h
View File

@@ -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

42
source/blender/physics/intern/eigen_utils.h
View File

@@ -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");

264
source/blender/physics/intern/hair_volume.cpp
View File

@@ -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

2
source/blender/physics/intern/implicit.h
View File

@@ -62,7 +62,7 @@ struct Implicit_Data;
typedef struct ImplicitSolverResult {
int status;
int iterations;
float error;
} ImplicitSolverResult;

436
source/blender/physics/intern/implicit_blender.c
View File

File diff suppressed because it is too large Load Diff

326
source/blender/physics/intern/implicit_eigen.cpp
View File

@@ -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;
}
}