diff options
author | Lukas Tönne <lukas.toenne@gmail.com> | 2014-09-12 12:26:43 +0400 |
---|---|---|
committer | Lukas Tönne <lukas.toenne@gmail.com> | 2015-01-20 11:29:59 +0300 |
commit | c5038b12c3d06d34c121412ceac14c08dda75bb2 (patch) | |
tree | 83ad3a3683f3bd5422cdfc911ac3f0a83271a5fd /source/blender/blenkernel/intern/implicit.c | |
parent | 3ad7974373dedd53b0d1890285a29399589b0a3a (diff) |
Ported root frame transformation to the old cloth solver.
Diffstat (limited to 'source/blender/blenkernel/intern/implicit.c')
-rw-r--r-- | source/blender/blenkernel/intern/implicit.c | 273 |
1 files changed, 247 insertions, 26 deletions
diff --git a/source/blender/blenkernel/intern/implicit.c b/source/blender/blenkernel/intern/implicit.c index c401eefa7dd..78f615e9a67 100644 --- a/source/blender/blenkernel/intern/implicit.c +++ b/source/blender/blenkernel/intern/implicit.c @@ -646,16 +646,28 @@ DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, flo /////////////////////////////////////////////////////////////////// // simulator start /////////////////////////////////////////////////////////////////// +typedef struct RootTransform { + float loc[3]; + float rot[3][3]; + + float vel[3]; + float omega[3]; + + float acc[3]; + float domega_dt[3]; +} RootTransform; + typedef struct Implicit_Data { /* inputs */ fmatrix3x3 *bigI; /* identity (constant) */ fmatrix3x3 *M; /* masses */ + lfVector *F; /* forces */ fmatrix3x3 *dFdV, *dFdX; /* force jacobians */ + RootTransform *root; /* root transforms */ /* motion state data */ lfVector *X, *Xnew; /* positions */ lfVector *V, *Vnew; /* velocities */ - lfVector *F; /* forces */ /* internal solver data */ lfVector *B; /* B for A*dV = B */ @@ -707,6 +719,8 @@ int implicit_init(Object *UNUSED(ob), ClothModifierData *clmd) id->dV = create_lfvector(cloth->numverts); id->z = create_lfvector(cloth->numverts); + id->root = MEM_callocN(sizeof(RootTransform) * cloth->numverts, "root transforms"); + for (i = 0; i < cloth->numverts; i++) { id->A[i].r = id->A[i].c = id->dFdV[i].r = id->dFdV[i].c = id->dFdX[i].r = id->dFdX[i].c = id->P[i].c = id->P[i].r = id->Pinv[i].c = id->Pinv[i].r = id->bigI[i].c = id->bigI[i].r = id->M[i].r = id->M[i].c = i; @@ -768,6 +782,8 @@ int implicit_free(ClothModifierData *clmd) del_lfvector(id->dV); del_lfvector(id->z); + MEM_freeN(id->root); + MEM_freeN(id); } } @@ -775,6 +791,179 @@ int implicit_free(ClothModifierData *clmd) return 1; } +/* ==== Transformation of Moving Reference Frame ==== + * x_world, v_world, f_world, a_world, dfdx_world, dfdv_world : state variables in world space + * x_root, v_root, f_root, a_root, dfdx_root, dfdv_root : state variables in root space + * + * x0 : translation of the root frame (hair root location) + * v0 : linear velocity of the root frame + * a0 : acceleration of the root frame + * R : rotation matrix of the root frame + * w : angular velocity of the root frame + * dwdt : angular acceleration of the root frame + */ + +/* x_root = R^T * x_world */ +BLI_INLINE void loc_world_to_root(float r[3], const float v[3], const RootTransform *root) +{ + sub_v3_v3v3(r, v, root->loc); + mul_transposed_m3_v3((float (*)[3])root->rot, r); +} + +/* x_world = R * x_root */ +BLI_INLINE void loc_root_to_world(float r[3], const float v[3], const RootTransform *root) +{ + copy_v3_v3(r, v); + mul_m3_v3((float (*)[3])root->rot, r); + add_v3_v3(r, root->loc); +} + +/* v_root = cross(w, x_root) + R^T*(v_world - v0) */ +BLI_INLINE void vel_world_to_root(float r[3], const float x_root[3], const float v[3], const RootTransform *root) +{ + float angvel[3]; + cross_v3_v3v3(angvel, root->omega, x_root); + + sub_v3_v3v3(r, v, root->vel); + mul_transposed_m3_v3((float (*)[3])root->rot, r); + add_v3_v3(r, angvel); +} + +/* v_world = R*(v_root - cross(w, x_root)) + v0 */ +BLI_INLINE void vel_root_to_world(float r[3], const float x_root[3], const float v[3], const RootTransform *root) +{ + float angvel[3]; + cross_v3_v3v3(angvel, root->omega, x_root); + + sub_v3_v3v3(r, v, angvel); + mul_m3_v3((float (*)[3])root->rot, r); + add_v3_v3(r, root->vel); +} + +/* a_root = -cross(dwdt, x_root) - 2*cross(w, v_root) - cross(w, cross(w, x_root)) + R^T*(a_world - a0) */ +BLI_INLINE void force_world_to_root(float r[3], const float x_root[3], const float v_root[3], const float force[3], float mass, const RootTransform *root) +{ + float euler[3], coriolis[3], centrifugal[3], rotvel[3]; + + cross_v3_v3v3(euler, root->domega_dt, x_root); + cross_v3_v3v3(coriolis, root->omega, v_root); + mul_v3_fl(coriolis, 2.0f); + cross_v3_v3v3(rotvel, root->omega, x_root); + cross_v3_v3v3(centrifugal, root->omega, rotvel); + + madd_v3_v3v3fl(r, force, root->acc, mass); + mul_transposed_m3_v3((float (*)[3])root->rot, r); + madd_v3_v3fl(r, euler, mass); + madd_v3_v3fl(r, coriolis, mass); + madd_v3_v3fl(r, centrifugal, mass); +} + +/* a_world = R*[ a_root + cross(dwdt, x_root) + 2*cross(w, v_root) + cross(w, cross(w, x_root)) ] + a0 */ +BLI_INLINE void force_root_to_world(float r[3], const float x_root[3], const float v_root[3], const float force[3], float mass, const RootTransform *root) +{ + float euler[3], coriolis[3], centrifugal[3], rotvel[3]; + + cross_v3_v3v3(euler, root->domega_dt, x_root); + cross_v3_v3v3(coriolis, root->omega, v_root); + mul_v3_fl(coriolis, 2.0f); + cross_v3_v3v3(rotvel, root->omega, x_root); + cross_v3_v3v3(centrifugal, root->omega, rotvel); + + madd_v3_v3v3fl(r, force, euler, mass); + madd_v3_v3fl(r, coriolis, mass); + madd_v3_v3fl(r, centrifugal, mass); + mul_m3_v3((float (*)[3])root->rot, r); + madd_v3_v3fl(r, root->acc, mass); +} + +BLI_INLINE void acc_world_to_root(float r[3], const float x_root[3], const float v_root[3], const float acc[3], const RootTransform *root) +{ + force_world_to_root(r, x_root, v_root, acc, 1.0f, root); +} + +BLI_INLINE void acc_root_to_world(float r[3], const float x_root[3], const float v_root[3], const float acc[3], const RootTransform *root) +{ + force_root_to_world(r, x_root, v_root, acc, 1.0f, root); +} + +BLI_INLINE void cross_m3_v3m3(float r[3][3], const float v[3], float m[3][3]) +{ + cross_v3_v3v3(r[0], v, m[0]); + cross_v3_v3v3(r[1], v, m[1]); + cross_v3_v3v3(r[2], v, m[2]); +} + +BLI_INLINE void cross_v3_identity(float r[3][3], const float v[3]) +{ + r[0][0] = 0.0f; r[1][0] = v[2]; r[2][0] = -v[1]; + r[0][1] = -v[2]; r[1][1] = 0.0f; r[2][1] = v[0]; + r[0][2] = v[1]; r[1][2] = -v[0]; r[2][2] = 0.0f; +} + +/* dfdx_root = m*[ -cross(dwdt, I) - cross(w, cross(w, I)) ] + R^T*(dfdx_world) */ +BLI_INLINE void dfdx_world_to_root(float m[3][3], float dfdx[3][3], float mass, const RootTransform *root) +{ + float t[3][3], u[3][3]; + + copy_m3_m3(t, (float (*)[3])root->rot); + transpose_m3(t); + mul_m3_m3m3(m, t, dfdx); + + cross_v3_identity(t, root->domega_dt); + mul_m3_fl(t, mass); + sub_m3_m3m3(m, m, t); + + cross_v3_identity(u, root->omega); + cross_m3_v3m3(t, root->omega, u); + mul_m3_fl(t, mass); + sub_m3_m3m3(m, m, t); +} + +/* dfdx_world = R*(dfdx_root + m*[ cross(dwdt, I) + cross(w, cross(w, I)) ]) */ +BLI_INLINE void dfdx_root_to_world(float m[3][3], float dfdx[3][3], float mass, const RootTransform *root) +{ + float t[3][3], u[3][3]; + + cross_v3_identity(t, root->domega_dt); + mul_m3_fl(t, mass); + add_m3_m3m3(m, dfdx, t); + + cross_v3_identity(u, root->omega); + cross_m3_v3m3(t, root->omega, u); + mul_m3_fl(t, mass); + add_m3_m3m3(m, m, t); + + mul_m3_m3m3(m, (float (*)[3])root->rot, m); +} + +/* dfdv_root = -2*m*cross(w, I) + R^T*(dfdv_world) */ +BLI_INLINE void dfdv_world_to_root(float m[3][3], float dfdv[3][3], float mass, const RootTransform *root) +{ + float t[3][3]; + + copy_m3_m3(t, (float (*)[3])root->rot); + transpose_m3(t); + mul_m3_m3m3(m, t, dfdv); + + cross_v3_identity(t, root->omega); + mul_m3_fl(t, 2.0f*mass); + sub_m3_m3m3(m, m, t); +} + +/* dfdv_world = R*(dfdv_root + 2*m*cross(w, I)) */ +BLI_INLINE void dfdv_root_to_world(float m[3][3], float dfdv[3][3], float mass, const RootTransform *root) +{ + float t[3][3]; + + cross_v3_identity(t, root->omega); + mul_m3_fl(t, 2.0f*mass); + add_m3_m3m3(m, dfdv, t); + + mul_m3_m3m3(m, (float (*)[3])root->rot, m); +} + +/* ================================ */ + DO_INLINE float fb(float length, float L) { float x = length / L; @@ -1904,8 +2093,10 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec { /* Collect forces and derivatives: F, dFdX, dFdV */ Cloth *cloth = clmd->clothObject; + ClothVertex *verts = cloth->verts; + RootTransform *roots = cloth->implicit->root; unsigned int i = 0; - float spring_air = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */ + float drag = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */ float gravity[3] = {0.0f, 0.0f, 0.0f}; MFace *mfaces = cloth->mfaces; unsigned int numverts = cloth->numverts; @@ -1913,7 +2104,8 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec lfVector *winvec; EffectedPoint epoint; - /* set dFdX jacobi matrix to zero */ + /* initialize forces to zero */ + zero_lfvector(lF, numverts); init_bfmatrix(dFdX, ZERO); init_bfmatrix(dFdV, ZERO); @@ -1923,14 +2115,14 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec copy_v3_v3(gravity, clmd->scene->physics_settings.gravity); mul_fvector_S(gravity, gravity, 0.001f * clmd->sim_parms->effector_weights->global_gravity); /* scale gravity force */ } - init_lfvector(lF, gravity, numverts); /* multiply lF with mass matrix * force = mass * acceleration (in this case: gravity) */ for (i = 0; i < numverts; i++) { - float temp[3]; - copy_v3_v3(temp, lF[i]); - mul_fmatrix_fvector(lF[i], M[i].m, temp); + float g[3]; + acc_world_to_root(g, lX[i], lV[i], gravity, &roots[i]); + mul_m3_v3(M[i].m, g); + add_v3_v3(lF[i], g); } #else zero_lfvector(lF, numverts); @@ -1941,11 +2133,34 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec #ifdef CLOTH_FORCE_DRAG /* set dFdX jacobi matrix diagonal entries to -spring_air */ for (i = 0; i < numverts; i++) { - dFdV[i].m[0][0] -= spring_air; - dFdV[i].m[1][1] -= spring_air; - dFdV[i].m[2][2] -= spring_air; + dFdV[i].m[0][0] -= drag; + dFdV[i].m[1][1] -= drag; + dFdV[i].m[2][2] -= drag; + } + submul_lfvectorS(lF, lV, drag, numverts); + for (i = 0; i < numverts; i++) { +#if 1 + float tmp[3][3]; + + /* NB: uses root space velocity, no need to transform */ + madd_v3_v3fl(lF[i], lV[i], -drag); + + copy_m3_m3(tmp, I); + mul_m3_fl(tmp, -drag); + add_m3_m3m3(dFdV[i].m, dFdV[i].m, tmp); +#else + float f[3], tmp[3][3], drag_dfdv[3][3], t[3]; + + mul_v3_v3fl(f, lV[i], -drag); + force_world_to_root(t, lX[i], lV[i], f, verts[i].mass, &roots[i]); + add_v3_v3(lF[i], t); + + copy_m3_m3(drag_dfdv, I); + mul_m3_fl(drag_dfdv, -drag); + dfdv_world_to_root(tmp, drag_dfdv, verts[i].mass, &roots[i]); + add_m3_m3m3(dFdV[i].m, dFdV[i].m, tmp); +#endif } - submul_lfvectorS(lF, lV, spring_air, numverts); #endif /* handle external forces like wind */ @@ -2186,8 +2401,10 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase * for (i = 0; i < numverts; i++) { // update velocities with constrained velocities from pinned verts if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) { - sub_v3_v3v3(id->V[i], verts[i].xconst, verts[i].xold); + float v[3]; + sub_v3_v3v3(v, verts[i].xconst, verts[i].xold); // mul_v3_fl(id->V[i], clmd->sim_parms->stepsPerFrame); + vel_world_to_root(id->V[i], id->X[i], v, &id->root[i]); } } } @@ -2202,7 +2419,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase * /* copy velocities for collision */ for (i = 0; i < numverts; i++) { - copy_v3_v3(verts[i].tv, id->V[i]); + vel_root_to_world(verts[i].tv, id->X[i], id->V[i], &id->root[i]); copy_v3_v3(verts[i].v, verts[i].tv); } @@ -2218,7 +2435,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase * // damping velocity for artistic reasons mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts); - + // calculate forces cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M); @@ -2232,15 +2449,13 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase * for (i = 0; i < numverts; i++) { if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) { - float tvect[3] = {0.0f, 0.0f, 0.0f}; - sub_v3_v3v3(tvect, verts[i].xconst, verts[i].xold); - mul_fvector_S(tvect, tvect, step+dt); - VECADD(tvect, tvect, verts[i].xold); - copy_v3_v3(id->Xnew[i], tvect); + float x[3]; + interp_v3_v3v3(x, verts[i].xold, verts[i].xconst, step + dt); + loc_world_to_root(id->Xnew[i], x, &id->root[i]); } } - copy_v3_v3(verts[i].txold, id->X[i]); + loc_root_to_world(verts[i].txold, id->X[i], &id->root[i]); if (!(verts[i].flags & CLOTH_VERT_FLAG_PINNED) && i > 0) { BKE_sim_debug_data_add_line(clmd->debug_data, id->X[i], id->X[i-1], 0.6, 0.3, 0.3, "hair", hash_vertex(4892, i)); @@ -2341,12 +2556,14 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase * if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED)) { copy_v3_v3(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x copy_v3_v3(verts[i].x, verts[i].xconst); - copy_v3_v3(verts[i].v, id->V[i]); + + vel_root_to_world(verts[i].v, id->X[i], id->V[i], &id->root[i]); } else { - copy_v3_v3(verts[i].txold, id->X[i]); - copy_v3_v3(verts[i].x, id->X[i]); - copy_v3_v3(verts[i].v, id->V[i]); + loc_root_to_world(verts[i].txold, id->X[i], &id->root[i]); + copy_v3_v3(verts[i].x, verts[i].txold); + + vel_root_to_world(verts[i].v, id->X[i], id->V[i], &id->root[i]); } } @@ -2361,11 +2578,15 @@ void implicit_set_positions(ClothModifierData *clmd) Cloth *cloth = clmd->clothObject; ClothVertex *verts = cloth->verts; unsigned int numverts = cloth->numverts, i; + ClothHairRoot *cloth_roots = clmd->roots; Implicit_Data *id = cloth->implicit; for (i = 0; i < numverts; i++) { - copy_v3_v3(id->X[i], verts[i].x); - copy_v3_v3(id->V[i], verts[i].v); + copy_v3_v3(id->root[i].loc, cloth_roots[i].loc); + copy_m3_m3(id->root[i].rot, cloth_roots[i].rot); + + loc_world_to_root(id->X[i], verts[i].x, &id->root[i]); + vel_world_to_root(id->V[i], id->X[i], verts[i].v, &id->root[i]); } if (G.debug_value > 0) printf("implicit_set_positions\n"); |