Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/source/blender/physics/intern/implicit_eigen.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'source/blender/physics/intern/implicit_eigen.cpp')
-rw-r--r--source/blender/physics/intern/implicit_eigen.cpp326
1 files changed, 163 insertions, 163 deletions
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;
}
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-13 05:55:34 +0300