Blender V5.3
implicit_eigen.cc
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2015-2023 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
8
9#ifdef IMPLICIT_SOLVER_EIGEN
10
11// #define USE_EIGEN_CORE
12# define USE_EIGEN_CONSTRAINED_CG
13
14# ifdef __GNUC__
15# pragma GCC diagnostic push
16/* XXX suppress verbose warnings in eigen */
17// # pragma GCC diagnostic ignored "-Wlogical-op"
18# endif
19
20# ifndef IMPLICIT_ENABLE_EIGEN_DEBUG
21# ifdef NDEBUG
22# define IMPLICIT_NDEBUG
23# endif
24# define NDEBUG
25# endif
26
27# include <Eigen/Sparse>
28# include <Eigen/src/Core/util/DisableStupidWarnings.h>
29
30# ifdef USE_EIGEN_CONSTRAINED_CG
32# endif
33
34# ifndef IMPLICIT_ENABLE_EIGEN_DEBUG
35# ifndef IMPLICIT_NDEBUG
36# undef NDEBUG
37# else
38# undef IMPLICIT_NDEBUG
39# endif
40# endif
41
42# ifdef __GNUC__
43# pragma GCC diagnostic pop
44# endif
45
46# include "MEM_guardedalloc.h"
47
48extern "C" {
50# include "DNA_object_types.h"
51# include "DNA_scene_types.h"
52# include "DNA_texture_types.h"
53
54# include "BLI_linklist.hh"
55# include "BLI_utildefines.hh"
56
57# include "BKE_cloth.hh"
58# include "BKE_collision.h"
59# include "BKE_effect.h"
60# include "BKE_global.hh"
61
62# include "SIM_mass_spring.h"
63}
64
65typedef float Scalar;
66
67static float I[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
68
69/* slightly extended Eigen vector class
70 * with conversion to/from plain C float array
71 */
72class fVector : public Eigen::Vector3f {
73 public:
74 typedef float *ctype;
75
76 fVector() {}
77
78 fVector(const ctype &v)
79 {
80 for (int k = 0; k < 3; k++) {
81 coeffRef(k) = v[k];
82 }
83 }
84
85 fVector &operator=(const ctype &v)
86 {
87 for (int k = 0; k < 3; k++) {
88 coeffRef(k) = v[k];
89 }
90 return *this;
91 }
92
93 operator ctype()
94 {
95 return data();
96 }
97};
98
99/* slightly extended Eigen matrix class
100 * with conversion to/from plain C float array
101 */
102class fMatrix : public Eigen::Matrix3f {
103 public:
104 typedef float (*ctype)[3];
105
106 fMatrix() {}
107
108 fMatrix(const ctype &v)
109 {
110 for (int k = 0; k < 3; k++) {
111 for (int l = 0; l < 3; l++) {
112 coeffRef(l, k) = v[k][l];
113 }
114 }
115 }
116
117 fMatrix &operator=(const ctype &v)
118 {
119 for (int k = 0; k < 3; k++) {
120 for (int l = 0; l < 3; l++) {
121 coeffRef(l, k) = v[k][l];
122 }
123 }
124 return *this;
125 }
126
127 operator ctype()
128 {
129 return (ctype)data();
130 }
131};
132
137class lVector : public Eigen::VectorXf {
138 public:
139 typedef Eigen::VectorXf base_t;
140
141 lVector() {}
142
143 template<typename T> lVector &operator=(T rhs)
144 {
145 base_t::operator=(rhs);
146 return *this;
147 }
148
149 float *v3(int vertex)
150 {
151 return &coeffRef(3 * vertex);
152 }
153
154 const float *v3(int vertex) const
155 {
156 return &coeffRef(3 * vertex);
157 }
158};
159
160typedef Eigen::Triplet<Scalar> Triplet;
161typedef std::vector<Triplet> TripletList;
162
163typedef Eigen::SparseMatrix<Scalar> lMatrix;
164
165/* Constructor type that provides more convenient handling of Eigen triplets
166 * for efficient construction of sparse 3x3 block matrices.
167 * This should be used for building lMatrix instead of writing to such lMatrix directly (which is
168 * very inefficient). After all elements have been defined using the set() method, the actual
169 * matrix can be filled using construct().
170 */
171struct lMatrixCtor {
172 lMatrixCtor() {}
173
174 void reset()
175 {
176 m_trips.clear();
177 }
178
179 void reserve(int numverts)
180 {
181 /* reserve for diagonal entries */
182 m_trips.reserve(numverts * 9);
183 }
184
185 void add(int i, int j, const fMatrix &m)
186 {
187 i *= 3;
188 j *= 3;
189 for (int k = 0; k < 3; k++) {
190 for (int l = 0; l < 3; l++) {
191 m_trips.push_back(Triplet(i + k, j + l, m.coeff(l, k)));
192 }
193 }
194 }
195
196 void sub(int i, int j, const fMatrix &m)
197 {
198 i *= 3;
199 j *= 3;
200 for (int k = 0; k < 3; k++) {
201 for (int l = 0; l < 3; l++) {
202 m_trips.push_back(Triplet(i + k, j + l, -m.coeff(l, k)));
203 }
204 }
205 }
206
207 inline void construct(lMatrix &m)
208 {
209 m.setFromTriplets(m_trips.begin(), m_trips.end());
210 m_trips.clear();
211 }
212
213 private:
214 TripletList m_trips;
215};
216
217# ifdef USE_EIGEN_CORE
218typedef Eigen::ConjugateGradient<lMatrix, Eigen::Lower, Eigen::DiagonalPreconditioner<Scalar>>
220# endif
221# ifdef USE_EIGEN_CONSTRAINED_CG
222typedef Eigen::ConstrainedConjugateGradient<lMatrix,
223 Eigen::Lower,
224 lMatrix,
225 Eigen::DiagonalPreconditioner<Scalar>>
226 ConstraintConjGrad;
227# endif
228using Eigen::ComputationInfo;
229
230static void print_lvector(const lVector &v)
231{
232 for (int i = 0; i < v.rows(); i++) {
233 if (i > 0 && i % 3 == 0) {
234 printf("\n");
235 }
236
237 printf("%f,\n", v[i]);
238 }
239}
240
241static void print_lmatrix(const lMatrix &m)
242{
243 for (int j = 0; j < m.rows(); j++) {
244 if (j > 0 && j % 3 == 0) {
245 printf("\n");
246 }
247
248 for (int i = 0; i < m.cols(); i++) {
249 if (i > 0 && i % 3 == 0) {
250 printf(" ");
251 }
252
253 implicit_print_matrix_elem(m.coeff(j, i));
254 }
255 printf("\n");
256 }
257}
258
259BLI_INLINE void lMatrix_reserve_elems(lMatrix &m, int num)
260{
261 m.reserve(Eigen::VectorXi::Constant(m.cols(), num));
262}
263
264BLI_INLINE float *lVector_v3(lVector &v, int vertex)
265{
266 return v.data() + 3 * vertex;
267}
268
269BLI_INLINE const float *lVector_v3(const lVector &v, int vertex)
270{
271 return v.data() + 3 * vertex;
272}
273
274# if 0
275BLI_INLINE void triplets_m3(TripletList &tlist, float m[3][3], int i, int j)
276{
277 i *= 3;
278 j *= 3;
279 for (int l = 0; l < 3; l++) {
280 for (int k = 0; k < 3; k++) {
281 tlist.push_back(Triplet(i + k, j + l, m[k][l]));
282 }
283 }
284}
285
286BLI_INLINE void triplets_m3fl(TripletList &tlist, float m[3][3], int i, int j, float factor)
287{
288 i *= 3;
289 j *= 3;
290 for (int l = 0; l < 3; l++) {
291 for (int k = 0; k < 3; k++) {
292 tlist.push_back(Triplet(i + k, j + l, m[k][l] * factor));
293 }
294 }
295}
296
297BLI_INLINE void lMatrix_add_triplets(lMatrix &r, const TripletList &tlist)
298{
299 lMatrix t(r.rows(), r.cols());
300 t.setFromTriplets(tlist.begin(), tlist.end());
301 r += t;
302}
303
304BLI_INLINE void lMatrix_madd_triplets(lMatrix &r, const TripletList &tlist, float f)
305{
306 lMatrix t(r.rows(), r.cols());
307 t.setFromTriplets(tlist.begin(), tlist.end());
308 r += f * t;
309}
310
311BLI_INLINE void lMatrix_sub_triplets(lMatrix &r, const TripletList &tlist)
312{
313 lMatrix t(r.rows(), r.cols());
314 t.setFromTriplets(tlist.begin(), tlist.end());
315 r -= t;
316}
317# endif
318
319BLI_INLINE void outerproduct(float r[3][3], const float a[3], const float b[3])
320{
321 mul_v3_v3fl(r[0], a, b[0]);
322 mul_v3_v3fl(r[1], a, b[1]);
323 mul_v3_v3fl(r[2], a, b[2]);
324}
325
326BLI_INLINE void cross_m3_v3m3(float r[3][3], const float v[3], float m[3][3])
327{
328 cross_v3_v3v3(r[0], v, m[0]);
329 cross_v3_v3v3(r[1], v, m[1]);
330 cross_v3_v3v3(r[2], v, m[2]);
331}
332
333BLI_INLINE void cross_v3_identity(float r[3][3], const float v[3])
334{
335 r[0][0] = 0.0f;
336 r[1][0] = v[2];
337 r[2][0] = -v[1];
338 r[0][1] = -v[2];
339 r[1][1] = 0.0f;
340 r[2][1] = v[0];
341 r[0][2] = v[1];
342 r[1][2] = -v[0];
343 r[2][2] = 0.0f;
344}
345
346BLI_INLINE void madd_m3_m3fl(float r[3][3], float m[3][3], float f)
347{
348 r[0][0] += m[0][0] * f;
349 r[0][1] += m[0][1] * f;
350 r[0][2] += m[0][2] * f;
351 r[1][0] += m[1][0] * f;
352 r[1][1] += m[1][1] * f;
353 r[1][2] += m[1][2] * f;
354 r[2][0] += m[2][0] * f;
355 r[2][1] += m[2][1] * f;
356 r[2][2] += m[2][2] * f;
357}
358
359BLI_INLINE void madd_m3_m3m3fl(float r[3][3], float a[3][3], float b[3][3], float f)
360{
361 r[0][0] = a[0][0] + b[0][0] * f;
362 r[0][1] = a[0][1] + b[0][1] * f;
363 r[0][2] = a[0][2] + b[0][2] * f;
364 r[1][0] = a[1][0] + b[1][0] * f;
365 r[1][1] = a[1][1] + b[1][1] * f;
366 r[1][2] = a[1][2] + b[1][2] * f;
367 r[2][0] = a[2][0] + b[2][0] * f;
368 r[2][1] = a[2][1] + b[2][1] * f;
369 r[2][2] = a[2][2] + b[2][2] * f;
370}
371
372struct Implicit_Data {
373 typedef std::vector<fMatrix> fMatrixVector;
374
375 Implicit_Data(int numverts)
376 {
377 resize(numverts);
378 }
379
380 void resize(int numverts)
381 {
382 this->numverts = numverts;
383 int tot = 3 * numverts;
384
385 M.resize(tot, tot);
386 F.resize(tot);
387 dFdX.resize(tot, tot);
388 dFdV.resize(tot, tot);
389
390 tfm.resize(numverts, I);
391
392 X.resize(tot);
393 Xnew.resize(tot);
394 V.resize(tot);
395 Vnew.resize(tot);
396
397 A.resize(tot, tot);
398 B.resize(tot);
399
400 dV.resize(tot);
401 z.resize(tot);
402 S.resize(tot, tot);
403
404 iM.reserve(numverts);
405 idFdX.reserve(numverts);
406 idFdV.reserve(numverts);
407 iS.reserve(numverts);
408 }
409
410 int numverts;
411
412 /* inputs */
413 lMatrix M; /* masses */
414 lVector F; /* forces */
415 lMatrix dFdX, dFdV; /* force jacobians */
416
417 fMatrixVector tfm; /* local coordinate transform */
418
419 /* motion state data */
420 lVector X, Xnew; /* positions */
421 lVector V, Vnew; /* velocities */
422
423 /* internal solver data */
424 lVector B; /* B for A*dV = B */
425 lMatrix A; /* A for A*dV = B */
426
427 lVector dV; /* velocity change (solution of A*dV = B) */
428 lVector z; /* target velocity in constrained directions */
429 lMatrix S; /* filtering matrix for constraints */
430
431 /* temporary constructors */
432 lMatrixCtor iM; /* masses */
433 lMatrixCtor idFdX, idFdV; /* force jacobians */
434 lMatrixCtor iS; /* filtering matrix for constraints */
435};
436
437Implicit_Data *SIM_mass_spring_solver_create(int numverts, int numsprings)
438{
439 Implicit_Data *id = new Implicit_Data(numverts);
440 return id;
441}
442
443void SIM_mass_spring_solver_free(Implicit_Data *id)
444{
445 if (id) {
446 delete id;
447 }
448}
449
450int SIM_mass_spring_solver_numvert(Implicit_Data *id)
451{
452 if (id) {
453 return id->numverts;
454 }
455 else {
456 return 0;
457 }
458}
459
460/* ==== Transformation from/to root reference frames ==== */
461
462BLI_INLINE void world_to_root_v3(Implicit_Data *data, int index, float r[3], const float v[3])
463{
464 copy_v3_v3(r, v);
466}
467
468BLI_INLINE void root_to_world_v3(Implicit_Data *data, int index, float r[3], const float v[3])
469{
470 mul_v3_m3v3(r, data->tfm[index], v);
471}
472
473BLI_INLINE void world_to_root_m3(Implicit_Data *data, int index, float r[3][3], float m[3][3])
474{
475 float trot[3][3];
476 copy_m3_m3(trot, data->tfm[index]);
477 transpose_m3(trot);
478 mul_m3_m3m3(r, trot, m);
479}
480
481BLI_INLINE void root_to_world_m3(Implicit_Data *data, int index, float r[3][3], float m[3][3])
482{
483 mul_m3_m3m3(r, data->tfm[index], m);
484}
485
486/* ================================ */
487
488bool SIM_mass_spring_solve_velocities(Implicit_Data *data, float dt, ImplicitSolverResult *result)
489{
490# ifdef USE_EIGEN_CORE
491 typedef ConjugateGradient solver_t;
492# endif
493# ifdef USE_EIGEN_CONSTRAINED_CG
494 typedef ConstraintConjGrad solver_t;
495# endif
496
497 data->iM.construct(data->M);
498 data->idFdX.construct(data->dFdX);
499 data->idFdV.construct(data->dFdV);
500 data->iS.construct(data->S);
501
502 solver_t cg;
503 cg.setMaxIterations(100);
504 cg.setTolerance(0.01f);
505
506# ifdef USE_EIGEN_CONSTRAINED_CG
507 cg.filter() = data->S;
508# endif
509
510 data->A = data->M - dt * data->dFdV - dt * dt * data->dFdX;
511 cg.compute(data->A);
512
513 data->B = dt * data->F + dt * dt * data->dFdX * data->V;
514
515# ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT
516 printf("==== A ====\n");
517 print_lmatrix(id->A);
518 printf("==== z ====\n");
519 print_lvector(id->z);
520 printf("==== B ====\n");
521 print_lvector(id->B);
522 printf("==== S ====\n");
523 print_lmatrix(id->S);
524# endif
525
526# ifdef USE_EIGEN_CORE
527 data->dV = cg.solve(data->B);
528# endif
529# ifdef USE_EIGEN_CONSTRAINED_CG
530 data->dV = cg.solveWithGuess(data->B, data->z);
531# endif
532
533# ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT
534 printf("==== dV ====\n");
535 print_lvector(id->dV);
536 printf("========\n");
537# endif
538
539 data->Vnew = data->V + data->dV;
540
541 switch (cg.info()) {
542 case Eigen::Success:
543 result->status = SIM_SOLVER_SUCCESS;
544 break;
545 case Eigen::NoConvergence:
547 break;
548 case Eigen::InvalidInput:
550 break;
551 case Eigen::NumericalIssue:
553 break;
554 }
555
556 result->iterations = cg.iterations();
557 result->error = cg.error();
558
559 return cg.info() == Eigen::Success;
560}
561
562bool SIM_mass_spring_solve_positions(Implicit_Data *data, float dt)
563{
564 data->Xnew = data->X + data->Vnew * dt;
565 return true;
566}
567
568/* ================================ */
569
570void SIM_mass_spring_apply_result(Implicit_Data *data)
571{
572 data->X = data->Xnew;
573 data->V = data->Vnew;
574}
575
576void SIM_mass_spring_set_vertex_mass(Implicit_Data *data, int index, float mass)
577{
578 float m[3][3];
579 copy_m3_m3(m, I);
580 mul_m3_fl(m, mass);
581 data->iM.add(index, index, m);
582}
583
584void SIM_mass_spring_set_rest_transform(Implicit_Data *data, int index, float tfm[3][3])
585{
586# ifdef CLOTH_ROOT_FRAME
587 copy_m3_m3(data->tfm[index], tfm);
588# else
589 unit_m3(data->tfm[index]);
590 (void)tfm;
591# endif
592}
593
594void SIM_mass_spring_set_motion_state(Implicit_Data *data,
595 int index,
596 const float x[3],
597 const float v[3])
598{
599 world_to_root_v3(data, index, data->X.v3(index), x);
600 world_to_root_v3(data, index, data->V.v3(index), v);
601}
602
603void SIM_mass_spring_set_position(Implicit_Data *data, int index, const float x[3])
604{
605 world_to_root_v3(data, index, data->X.v3(index), x);
606}
607
608void SIM_mass_spring_set_velocity(Implicit_Data *data, int index, const float v[3])
609{
610 world_to_root_v3(data, index, data->V.v3(index), v);
611}
612
613void SIM_mass_spring_get_motion_state(struct Implicit_Data *data,
614 int index,
615 float x[3],
616 float v[3])
617{
618 if (x) {
619 root_to_world_v3(data, index, x, data->X.v3(index));
620 }
621 if (v) {
622 root_to_world_v3(data, index, v, data->V.v3(index));
623 }
624}
625
626void SIM_mass_spring_get_position(struct Implicit_Data *data, int index, float x[3])
627{
628 root_to_world_v3(data, index, x, data->X.v3(index));
629}
630
631void SIM_mass_spring_get_new_velocity(Implicit_Data *data, int index, float v[3])
632{
633 root_to_world_v3(data, index, v, data->V.v3(index));
634}
635
636void SIM_mass_spring_set_new_velocity(Implicit_Data *data, int index, const float v[3])
637{
638 world_to_root_v3(data, index, data->V.v3(index), v);
639}
640
641void SIM_mass_spring_clear_constraints(Implicit_Data *data)
642{
643 int numverts = data->numverts;
644 for (int i = 0; i < numverts; i++) {
645 data->iS.add(i, i, I);
646 zero_v3(data->z.v3(i));
647 }
648}
649
650void SIM_mass_spring_add_constraint_ndof0(Implicit_Data *data, int index, const float dV[3])
651{
652 data->iS.sub(index, index, I);
653
654 world_to_root_v3(data, index, data->z.v3(index), dV);
655}
656
658 Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3])
659{
660 float m[3][3], p[3], q[3], u[3], cmat[3][3];
661
662 world_to_root_v3(data, index, p, c1);
663 outerproduct(cmat, p, p);
664 copy_m3_m3(m, cmat);
665
666 world_to_root_v3(data, index, q, c2);
667 outerproduct(cmat, q, q);
668 add_m3_m3m3(m, m, cmat);
669
670 /* XXX not sure but multiplication should work here */
671 data->iS.sub(index, index, m);
672 // mul_m3_m3m3(data->S[index].m, data->S[index].m, m);
673
674 world_to_root_v3(data, index, u, dV);
675 add_v3_v3(data->z.v3(index), u);
676}
677
678void SIM_mass_spring_add_constraint_ndof2(Implicit_Data *data,
679 int index,
680 const float c1[3],
681 const float dV[3])
682{
683 float m[3][3], p[3], u[3], cmat[3][3];
684
685 world_to_root_v3(data, index, p, c1);
686 outerproduct(cmat, p, p);
687 copy_m3_m3(m, cmat);
688
689 data->iS.sub(index, index, m);
690 // mul_m3_m3m3(data->S[index].m, data->S[index].m, m);
691
692 world_to_root_v3(data, index, u, dV);
693 add_v3_v3(data->z.v3(index), u);
694}
695
696void SIM_mass_spring_clear_forces(Implicit_Data *data)
697{
698 data->F.setZero();
699 data->dFdX.setZero();
700 data->dFdV.setZero();
701}
702
703void SIM_mass_spring_force_reference_frame(Implicit_Data *data,
704 int index,
705 const float acceleration[3],
706 const float omega[3],
707 const float domega_dt[3],
708 float mass)
709{
710# ifdef CLOTH_ROOT_FRAME
711 float acc[3], w[3], dwdt[3];
712 float f[3], dfdx[3][3], dfdv[3][3];
713 float euler[3], coriolis[3], centrifugal[3], rotvel[3];
714 float deuler[3][3], dcoriolis[3][3], dcentrifugal[3][3], drotvel[3][3];
715
716 world_to_root_v3(data, index, acc, acceleration);
717 world_to_root_v3(data, index, w, omega);
718 world_to_root_v3(data, index, dwdt, domega_dt);
719
720 cross_v3_v3v3(euler, dwdt, data->X.v3(index));
721 cross_v3_v3v3(coriolis, w, data->V.v3(index));
722 mul_v3_fl(coriolis, 2.0f);
723 cross_v3_v3v3(rotvel, w, data->X.v3(index));
724 cross_v3_v3v3(centrifugal, w, rotvel);
725
726 sub_v3_v3v3(f, acc, euler);
727 sub_v3_v3(f, coriolis);
728 sub_v3_v3(f, centrifugal);
729
730 mul_v3_fl(f, mass); /* F = m * a */
731
732 cross_v3_identity(deuler, dwdt);
733 cross_v3_identity(dcoriolis, w);
734 mul_m3_fl(dcoriolis, 2.0f);
735 cross_v3_identity(drotvel, w);
736 cross_m3_v3m3(dcentrifugal, w, drotvel);
737
738 add_m3_m3m3(dfdx, deuler, dcentrifugal);
739 negate_m3(dfdx);
740 mul_m3_fl(dfdx, mass);
741
742 copy_m3_m3(dfdv, dcoriolis);
743 negate_m3(dfdv);
744 mul_m3_fl(dfdv, mass);
745
746 add_v3_v3(data->F.v3(index), f);
747 data->idFdX.add(index, index, dfdx);
748 data->idFdV.add(index, index, dfdv);
749# else
750 (void)data;
751 (void)index;
752 (void)acceleration;
753 (void)omega;
754 (void)domega_dt;
755# endif
756}
757
758void SIM_mass_spring_force_gravity(Implicit_Data *data, int index, float mass, const float g[3])
759{
760 /* force = mass * acceleration (in this case: gravity) */
761 float f[3];
762 world_to_root_v3(data, index, f, g);
763 mul_v3_fl(f, mass);
764
765 add_v3_v3(data->F.v3(index), f);
766}
767
768void SIM_mass_spring_force_drag(Implicit_Data *data, float drag)
769{
770 int numverts = data->numverts;
771 for (int i = 0; i < numverts; i++) {
772 float tmp[3][3];
773
774 /* NOTE: Uses root space velocity, no need to transform. */
775 madd_v3_v3fl(data->F.v3(i), data->V.v3(i), -drag);
776
777 copy_m3_m3(tmp, I);
778 mul_m3_fl(tmp, -drag);
779 data->idFdV.add(i, i, tmp);
780 }
781}
782
784 struct Implicit_Data *data, int i, const float f[3], float dfdx[3][3], float dfdv[3][3])
785{
786 float tf[3], tdfdx[3][3], tdfdv[3][3];
787 world_to_root_v3(data, i, tf, f);
788 world_to_root_m3(data, i, tdfdx, dfdx);
789 world_to_root_m3(data, i, tdfdv, dfdv);
790
791 add_v3_v3(data->F.v3(i), tf);
792 data->idFdX.add(i, i, tdfdx);
793 data->idFdV.add(i, i, tdfdv);
794}
795
796static float calc_nor_area_tri(float nor[3],
797 const float v1[3],
798 const float v2[3],
799 const float v3[3])
800{
801 float n1[3], n2[3];
802
803 sub_v3_v3v3(n1, v1, v2);
804 sub_v3_v3v3(n2, v2, v3);
805
806 cross_v3_v3v3(nor, n1, n2);
807 return normalize_v3(nor) / 2.0f;
808}
809
810/* XXX does not support force jacobians yet,
811 * since the effector system does not provide them either. */
813 Implicit_Data *data, int v1, int v2, int v3, const float (*winvec)[3])
814{
815 const float effector_scale = 0.02f;
816 float win[3], nor[3], area;
817 float factor;
818
819 /* calculate face normal and area */
820 area = calc_nor_area_tri(nor, data->X.v3(v1), data->X.v3(v2), data->X.v3(v3));
821 factor = effector_scale * area / 3.0f;
822
823 world_to_root_v3(data, v1, win, winvec[v1]);
824 madd_v3_v3fl(data->F.v3(v1), nor, factor * dot_v3v3(win, nor));
825
826 world_to_root_v3(data, v2, win, winvec[v2]);
827 madd_v3_v3fl(data->F.v3(v2), nor, factor * dot_v3v3(win, nor));
828
829 world_to_root_v3(data, v3, win, winvec[v3]);
830 madd_v3_v3fl(data->F.v3(v3), nor, factor * dot_v3v3(win, nor));
831}
832
833void SIM_mass_spring_force_edge_wind(Implicit_Data *data, int v1, int v2, const float (*winvec)[3])
834{
835 const float effector_scale = 0.01;
836 float win[3], dir[3], nor[3], length;
837
838 sub_v3_v3v3(dir, data->X.v3(v1), data->X.v3(v2));
839 length = normalize_v3(dir);
840
841 world_to_root_v3(data, v1, win, winvec[v1]);
842 madd_v3_v3v3fl(nor, win, dir, -dot_v3v3(win, dir));
843 madd_v3_v3fl(data->F.v3(v1), nor, effector_scale * length);
844
845 world_to_root_v3(data, v2, win, winvec[v2]);
846 madd_v3_v3v3fl(nor, win, dir, -dot_v3v3(win, dir));
847 madd_v3_v3fl(data->F.v3(v2), nor, effector_scale * length);
848}
849
850BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, float L, float k)
851{
852 /* dir is unit length direction, rest is spring's restlength, k is spring constant. */
853 // return ((I - outerprod(dir, dir)) * Min(1.0f, rest / length) - I) * -k;
854 outerproduct(to, dir, dir);
855 sub_m3_m3m3(to, I, to);
856
857 mul_m3_fl(to, (L / length));
858 sub_m3_m3m3(to, to, I);
859 mul_m3_fl(to, k);
860}
861
862/* unused */
863# if 0
864BLI_INLINE void dfdx_damp(float to[3][3],
865 const float dir[3],
866 float length,
867 const float vel[3],
868 float rest,
869 float damping)
870{
871 /* Inner spring damping vel is the relative velocity of the endpoints. */
872 // return (I - outerprod(dir, dir)) * (-damping * -(dot(dir, vel) / Max(length, rest)));
873 mul_fvectorT_fvector(to, dir, dir);
874 sub_fmatrix_fmatrix(to, I, to);
875 mul_fmatrix_S(to, (-damping * -(dot_v3v3(dir, vel) / std::max(length, rest))));
876}
877# endif
878
879BLI_INLINE void dfdv_damp(float to[3][3], const float dir[3], float damping)
880{
881 /* Derivative of force with regards to velocity. */
882 outerproduct(to, dir, dir);
883 mul_m3_fl(to, -damping);
884}
885
886BLI_INLINE float fb(float length, float L)
887{
888 float x = length / L;
889 return (-11.541f * powf(x, 4) + 34.193f * powf(x, 3) - 39.083f * powf(x, 2) + 23.116f * x -
890 9.713f);
891}
892
893BLI_INLINE float fbderiv(float length, float L)
894{
895 float x = length / L;
896
897 return (-46.164f * powf(x, 3) + 102.579f * powf(x, 2) - 78.166f * x + 23.116f);
898}
899
900BLI_INLINE float fbstar(float length, float L, float kb, float cb)
901{
902 float tempfb_fl = kb * fb(length, L);
903 float fbstar_fl = cb * (length - L);
904
905 if (tempfb_fl < fbstar_fl) {
906 return fbstar_fl;
907 }
908 else {
909 return tempfb_fl;
910 }
911}
912
913/* Function to calculate bending spring force (taken from Choi & Co). */
914BLI_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
915{
916 float tempfb_fl = kb * fb(length, L);
917 float fbstar_fl = cb * (length - L);
918
919 if (tempfb_fl < fbstar_fl) {
920 return -cb;
921 }
922 else {
923 return -kb * fbderiv(length, L);
924 }
925}
926
927/* calculate elongation */
928BLI_INLINE bool spring_length(Implicit_Data *data,
929 int i,
930 int j,
931 float r_extent[3],
932 float r_dir[3],
933 float *r_length,
934 float r_vel[3])
935{
936 sub_v3_v3v3(r_extent, data->X.v3(j), data->X.v3(i));
937 sub_v3_v3v3(r_vel, data->V.v3(j), data->V.v3(i));
938 *r_length = len_v3(r_extent);
939
940 if (*r_length > ALMOST_ZERO) {
941# if 0
942 if (length > L) {
943 if ((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) &&
944 (((length - L) * 100.0f / L) > clmd->sim_parms->maxspringlen))
945 {
946 /* cut spring! */
947 s->flags |= CSPRING_FLAG_DEACTIVATE;
948 return false;
949 }
950 }
951# endif
952 mul_v3_v3fl(r_dir, r_extent, 1.0f / (*r_length));
953 }
954 else {
955 zero_v3(r_dir);
956 }
957
958 return true;
959}
960
962 Implicit_Data *data, int i, int j, const float f[3], float dfdx[3][3], float dfdv[3][3])
963{
964 add_v3_v3(data->F.v3(i), f);
965 sub_v3_v3(data->F.v3(j), f);
966
967 data->idFdX.add(i, i, dfdx);
968 data->idFdX.add(j, j, dfdx);
969 data->idFdX.sub(i, j, dfdx);
970 data->idFdX.sub(j, i, dfdx);
971
972 data->idFdV.add(i, i, dfdv);
973 data->idFdV.add(j, j, dfdv);
974 data->idFdV.sub(i, j, dfdv);
975 data->idFdV.sub(j, i, dfdv);
976}
977
978bool SIM_mass_spring_force_spring_linear(Implicit_Data *data,
979 int i,
980 int j,
981 float restlen,
982 float stiffness,
983 float damping,
984 bool no_compress,
985 float clamp_force,
986 float r_f[3],
987 float r_dfdx[3][3],
988 float r_dfdv[3][3])
989{
990 float extent[3], length, dir[3], vel[3];
991
992 /* calculate elongation */
993 spring_length(data, i, j, extent, dir, &length, vel);
994
995 if (length > restlen || no_compress) {
996 float stretch_force, f[3], dfdx[3][3], dfdv[3][3];
997
998 stretch_force = stiffness * (length - restlen);
999 if (clamp_force > 0.0f && stretch_force > clamp_force) {
1000 stretch_force = clamp_force;
1001 }
1002 mul_v3_v3fl(f, dir, stretch_force);
1003
1004 /* Ascher & Boxman, p.21: Damping only during elongation
1005 * something wrong with it... */
1006 madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir));
1007
1008 dfdx_spring(dfdx, dir, length, restlen, stiffness);
1009 dfdv_damp(dfdv, dir, damping);
1010
1011 apply_spring(data, i, j, f, dfdx, dfdv);
1012
1013 if (r_f) {
1014 copy_v3_v3(r_f, f);
1015 }
1016 if (r_dfdx) {
1017 copy_m3_m3(r_dfdx, dfdx);
1018 }
1019 if (r_dfdv) {
1020 copy_m3_m3(r_dfdv, dfdv);
1021 }
1022
1023 return true;
1024 }
1025 else {
1026 if (r_f) {
1027 zero_v3(r_f);
1028 }
1029 if (r_dfdx) {
1030 zero_m3(r_dfdx);
1031 }
1032 if (r_dfdv) {
1033 zero_m3(r_dfdv);
1034 }
1035
1036 return false;
1037 }
1038}
1039
1040/* See "Stable but Responsive Cloth" (Choi, Ko 2005) */
1041bool SIM_mass_spring_force_spring_bending(Implicit_Data *data,
1042 int i,
1043 int j,
1044 float restlen,
1045 float kb,
1046 float cb,
1047 float r_f[3],
1048 float r_dfdx[3][3],
1049 float r_dfdv[3][3])
1050{
1051 float extent[3], length, dir[3], vel[3];
1052
1053 /* calculate elongation */
1054 spring_length(data, i, j, extent, dir, &length, vel);
1055
1056 if (length < restlen) {
1057 float f[3], dfdx[3][3], dfdv[3][3];
1058
1059 mul_v3_v3fl(f, dir, fbstar(length, restlen, kb, cb));
1060
1061 outerproduct(dfdx, dir, dir);
1062 mul_m3_fl(dfdx, fbstar_jacobi(length, restlen, kb, cb));
1063
1064 /* XXX damping not supported */
1065 zero_m3(dfdv);
1066
1067 apply_spring(data, i, j, f, dfdx, dfdv);
1068
1069 if (r_f) {
1070 copy_v3_v3(r_f, f);
1071 }
1072 if (r_dfdx) {
1073 copy_m3_m3(r_dfdx, dfdx);
1074 }
1075 if (r_dfdv) {
1076 copy_m3_m3(r_dfdv, dfdv);
1077 }
1078
1079 return true;
1080 }
1081 else {
1082 if (r_f) {
1083 zero_v3(r_f);
1084 }
1085 if (r_dfdx) {
1086 zero_m3(r_dfdx);
1087 }
1088 if (r_dfdv) {
1089 zero_m3(r_dfdv);
1090 }
1091
1092 return false;
1093 }
1094}
1095
1096/* Jacobian of a direction vector.
1097 * Basically the part of the differential orthogonal to the direction,
1098 * inversely proportional to the length of the edge.
1099 *
1100 * dD_ij/dx_i = -dD_ij/dx_j = (D_ij * D_ij^T - I) / len_ij
1101 */
1103 Implicit_Data *data, int i, int j, float edge[3], float dir[3], float grad_dir[3][3])
1104{
1105 float length;
1106
1107 sub_v3_v3v3(edge, data->X.v3(j), data->X.v3(i));
1108 length = normalize_v3_v3(dir, edge);
1109
1110 if (length > ALMOST_ZERO) {
1111 outerproduct(grad_dir, dir, dir);
1112 sub_m3_m3m3(grad_dir, I, grad_dir);
1113 mul_m3_fl(grad_dir, 1.0f / length);
1114 }
1115 else {
1116 zero_m3(grad_dir);
1117 }
1118}
1119
1120BLI_INLINE void spring_angbend_forces(Implicit_Data *data,
1121 int i,
1122 int j,
1123 int k,
1124 const float goal[3],
1125 float stiffness,
1126 float damping,
1127 int q,
1128 const float dx[3],
1129 const float dv[3],
1130 float r_f[3])
1131{
1132 float edge_ij[3], dir_ij[3];
1133 float edge_jk[3], dir_jk[3];
1134 float vel_ij[3], vel_jk[3], vel_ortho[3];
1135 float f_bend[3], f_damp[3];
1136 float fk[3];
1137 float dist[3];
1138
1139 zero_v3(fk);
1140
1141 sub_v3_v3v3(edge_ij, data->X.v3(j), data->X.v3(i));
1142 if (q == i) {
1143 sub_v3_v3(edge_ij, dx);
1144 }
1145 if (q == j) {
1146 add_v3_v3(edge_ij, dx);
1147 }
1148 normalize_v3_v3(dir_ij, edge_ij);
1149
1150 sub_v3_v3v3(edge_jk, data->X.v3(k), data->X.v3(j));
1151 if (q == j) {
1152 sub_v3_v3(edge_jk, dx);
1153 }
1154 if (q == k) {
1155 add_v3_v3(edge_jk, dx);
1156 }
1157 normalize_v3_v3(dir_jk, edge_jk);
1158
1159 sub_v3_v3v3(vel_ij, data->V.v3(j), data->V.v3(i));
1160 if (q == i) {
1161 sub_v3_v3(vel_ij, dv);
1162 }
1163 if (q == j) {
1164 add_v3_v3(vel_ij, dv);
1165 }
1166
1167 sub_v3_v3v3(vel_jk, data->V.v3(k), data->V.v3(j));
1168 if (q == j) {
1169 sub_v3_v3(vel_jk, dv);
1170 }
1171 if (q == k) {
1172 add_v3_v3(vel_jk, dv);
1173 }
1174
1175 /* bending force */
1176 sub_v3_v3v3(dist, goal, edge_jk);
1177 mul_v3_v3fl(f_bend, dist, stiffness);
1178
1179 add_v3_v3(fk, f_bend);
1180
1181 /* damping force */
1182 madd_v3_v3v3fl(vel_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk));
1183 mul_v3_v3fl(f_damp, vel_ortho, damping);
1184
1185 sub_v3_v3(fk, f_damp);
1186
1187 copy_v3_v3(r_f, fk);
1188}
1189
1190/* Finite Differences method for estimating the jacobian of the force */
1191BLI_INLINE void spring_angbend_estimate_dfdx(Implicit_Data *data,
1192 int i,
1193 int j,
1194 int k,
1195 const float goal[3],
1196 float stiffness,
1197 float damping,
1198 int q,
1199 float dfdx[3][3])
1200{
1201 const float delta = 0.00001f; /* TODO: find a good heuristic for this. */
1202 float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3];
1203 float f[3];
1204 int a, b;
1205
1206 zero_m3(dvec_null);
1207 unit_m3(dvec_pos);
1208 mul_m3_fl(dvec_pos, delta * 0.5f);
1209 copy_m3_m3(dvec_neg, dvec_pos);
1210 negate_m3(dvec_neg);
1211
1212 /* XXX TODO: offset targets to account for position dependency. */
1213
1214 for (a = 0; a < 3; a++) {
1215 spring_angbend_forces(
1216 data, i, j, k, goal, stiffness, damping, q, dvec_pos[a], dvec_null[a], f);
1217 copy_v3_v3(dfdx[a], f);
1218
1219 spring_angbend_forces(
1220 data, i, j, k, goal, stiffness, damping, q, dvec_neg[a], dvec_null[a], f);
1221 sub_v3_v3(dfdx[a], f);
1222
1223 for (b = 0; b < 3; b++) {
1224 dfdx[a][b] /= delta;
1225 }
1226 }
1227}
1228
1229/* Finite Differences method for estimating the jacobian of the force */
1230BLI_INLINE void spring_angbend_estimate_dfdv(Implicit_Data *data,
1231 int i,
1232 int j,
1233 int k,
1234 const float goal[3],
1235 float stiffness,
1236 float damping,
1237 int q,
1238 float dfdv[3][3])
1239{
1240 const float delta = 0.00001f; /* TODO: find a good heuristic for this. */
1241 float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3];
1242 float f[3];
1243 int a, b;
1244
1245 zero_m3(dvec_null);
1246 unit_m3(dvec_pos);
1247 mul_m3_fl(dvec_pos, delta * 0.5f);
1248 copy_m3_m3(dvec_neg, dvec_pos);
1249 negate_m3(dvec_neg);
1250
1251 /* XXX TODO: offset targets to account for position dependency. */
1252
1253 for (a = 0; a < 3; a++) {
1254 spring_angbend_forces(
1255 data, i, j, k, goal, stiffness, damping, q, dvec_null[a], dvec_pos[a], f);
1256 copy_v3_v3(dfdv[a], f);
1257
1258 spring_angbend_forces(
1259 data, i, j, k, goal, stiffness, damping, q, dvec_null[a], dvec_neg[a], f);
1260 sub_v3_v3(dfdv[a], f);
1261
1262 for (b = 0; b < 3; b++) {
1263 dfdv[a][b] /= delta;
1264 }
1265 }
1266}
1267
1268/* Angular spring that pulls the vertex toward the local target
1269 * See "Artistic Simulation of Curly Hair" (Pixar technical memo #12-03a)
1270 */
1271bool SIM_mass_spring_force_spring_bending_angular(Implicit_Data *data,
1272 int i,
1273 int j,
1274 int k,
1275 const float target[3],
1276 float stiffness,
1277 float damping)
1278{
1279 float goal[3];
1280 float fj[3], fk[3];
1281 float dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3];
1282 float dfj_dvi[3][3], dfj_dvj[3][3], dfk_dvi[3][3], dfk_dvj[3][3], dfk_dvk[3][3];
1283
1284 const float vecnull[3] = {0.0f, 0.0f, 0.0f};
1285
1286 world_to_root_v3(data, j, goal, target);
1287
1288 spring_angbend_forces(data, i, j, k, goal, stiffness, damping, k, vecnull, vecnull, fk);
1289 negate_v3_v3(fj, fk); /* counterforce */
1290
1291 spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, i, dfk_dxi);
1292 spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, j, dfk_dxj);
1293 spring_angbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, k, dfk_dxk);
1294 copy_m3_m3(dfj_dxi, dfk_dxi);
1295 negate_m3(dfj_dxi);
1296 copy_m3_m3(dfj_dxj, dfk_dxj);
1297 negate_m3(dfj_dxj);
1298
1299 spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, i, dfk_dvi);
1300 spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, j, dfk_dvj);
1301 spring_angbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, k, dfk_dvk);
1302 copy_m3_m3(dfj_dvi, dfk_dvi);
1303 negate_m3(dfj_dvi);
1304 copy_m3_m3(dfj_dvj, dfk_dvj);
1305 negate_m3(dfj_dvj);
1306
1307 /* add forces and jacobians to the solver data */
1308
1309 add_v3_v3(data->F.v3(j), fj);
1310 add_v3_v3(data->F.v3(k), fk);
1311
1312 data->idFdX.add(j, j, dfj_dxj);
1313 data->idFdX.add(k, k, dfk_dxk);
1314
1315 data->idFdX.add(i, j, dfj_dxi);
1316 data->idFdX.add(j, i, dfj_dxi);
1317 data->idFdX.add(j, k, dfk_dxj);
1318 data->idFdX.add(k, j, dfk_dxj);
1319 data->idFdX.add(i, k, dfk_dxi);
1320 data->idFdX.add(k, i, dfk_dxi);
1321
1322 data->idFdV.add(j, j, dfj_dvj);
1323 data->idFdV.add(k, k, dfk_dvk);
1324
1325 data->idFdV.add(i, j, dfj_dvi);
1326 data->idFdV.add(j, i, dfj_dvi);
1327 data->idFdV.add(j, k, dfk_dvj);
1328 data->idFdV.add(k, j, dfk_dvj);
1329 data->idFdV.add(i, k, dfk_dvi);
1330 data->idFdV.add(k, i, dfk_dvi);
1331
1332 /* XXX analytical calculation of derivatives below is incorrect.
1333 * This proved to be difficult, but for now just using the finite difference method for
1334 * estimating the jacobians should be sufficient.
1335 */
1336# if 0
1337 float edge_ij[3], dir_ij[3], grad_dir_ij[3][3];
1338 float edge_jk[3], dir_jk[3], grad_dir_jk[3][3];
1339 float dist[3], vel_jk[3], vel_jk_ortho[3], projvel[3];
1340 float target[3];
1341 float tmp[3][3];
1342 float fi[3], fj[3], fk[3];
1343 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];
1344 float dfdvi[3][3];
1345
1346 /* TESTING */
1347 damping = 0.0f;
1348
1349 zero_v3(fi);
1350 zero_v3(fj);
1351 zero_v3(fk);
1352 zero_m3(dfi_dxi);
1353 zero_m3(dfj_dxi);
1354 zero_m3(dfk_dxi);
1355 zero_m3(dfk_dxj);
1356 zero_m3(dfk_dxk);
1357
1358 /* jacobian of direction vectors */
1359 spring_grad_dir(data, i, j, edge_ij, dir_ij, grad_dir_ij);
1360 spring_grad_dir(data, j, k, edge_jk, dir_jk, grad_dir_jk);
1361
1362 sub_v3_v3v3(vel_jk, data->V[k], data->V[j]);
1363
1364 /* bending force */
1365 mul_v3_v3fl(target, dir_ij, restlen);
1366 sub_v3_v3v3(dist, target, edge_jk);
1367 mul_v3_v3fl(fk, dist, stiffness);
1368
1369 /* damping force */
1370 madd_v3_v3v3fl(vel_jk_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk));
1371 madd_v3_v3fl(fk, vel_jk_ortho, damping);
1372
1373 /* XXX this only holds true as long as we assume straight rest shape!
1374 * eventually will become a bit more involved since the opposite segment
1375 * gets its own target, under condition of having equal torque on both sides.
1376 */
1377 copy_v3_v3(fi, fk);
1378
1379 /* counterforce on the middle point */
1380 sub_v3_v3(fj, fi);
1381 sub_v3_v3(fj, fk);
1382
1383 /* === derivatives === */
1384
1385 madd_m3_m3fl(dfk_dxi, grad_dir_ij, stiffness * restlen);
1386
1387 madd_m3_m3fl(dfk_dxj, grad_dir_ij, -stiffness * restlen);
1388 madd_m3_m3fl(dfk_dxj, I, stiffness);
1389
1390 madd_m3_m3fl(dfk_dxk, I, -stiffness);
1391
1392 copy_m3_m3(dfi_dxi, dfk_dxk);
1393 negate_m3(dfi_dxi);
1394
1395 /* dfj_dfi == dfi_dfj due to symmetry,
1396 * dfi_dfj == dfk_dfj due to fi == fk
1397 * XXX see comment above on future bent rest shapes
1398 */
1399 copy_m3_m3(dfj_dxi, dfk_dxj);
1400
1401 /* dfj_dxj == -(dfi_dxj + dfk_dxj) due to fj == -(fi + fk) */
1402 sub_m3_m3m3(dfj_dxj, dfj_dxj, dfj_dxi);
1403 sub_m3_m3m3(dfj_dxj, dfj_dxj, dfk_dxj);
1404
1405 /* add forces and jacobians to the solver data */
1406 add_v3_v3(data->F[i], fi);
1407 add_v3_v3(data->F[j], fj);
1408 add_v3_v3(data->F[k], fk);
1409
1410 add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfi_dxi);
1411 add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfj_dxj);
1412 add_m3_m3m3(data->dFdX[k].m, data->dFdX[k].m, dfk_dxk);
1413
1414 add_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfj_dxi);
1415 add_m3_m3m3(data->dFdX[block_jk].m, data->dFdX[block_jk].m, dfk_dxj);
1416 add_m3_m3m3(data->dFdX[block_ik].m, data->dFdX[block_ik].m, dfk_dxi);
1417# endif
1418
1419 return true;
1420}
1421
1422bool SIM_mass_spring_force_spring_goal(Implicit_Data *data,
1423 int i,
1424 const float goal_x[3],
1425 const float goal_v[3],
1426 float stiffness,
1427 float damping,
1428 float r_f[3],
1429 float r_dfdx[3][3],
1430 float r_dfdv[3][3])
1431{
1432 float root_goal_x[3], root_goal_v[3], extent[3], length, dir[3], vel[3];
1433 float f[3], dfdx[3][3], dfdv[3][3];
1434
1435 /* goal is in world space */
1436 world_to_root_v3(data, i, root_goal_x, goal_x);
1437 world_to_root_v3(data, i, root_goal_v, goal_v);
1438
1439 sub_v3_v3v3(extent, root_goal_x, data->X.v3(i));
1440 sub_v3_v3v3(vel, root_goal_v, data->V.v3(i));
1441 length = normalize_v3_v3(dir, extent);
1442
1443 if (length > ALMOST_ZERO) {
1444 mul_v3_v3fl(f, dir, stiffness * length);
1445
1446 /* Ascher & Boxman, p.21: Damping only during elongation
1447 * something wrong with it... */
1448 madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir));
1449
1450 dfdx_spring(dfdx, dir, length, 0.0f, stiffness);
1451 dfdv_damp(dfdv, dir, damping);
1452
1453 add_v3_v3(data->F.v3(i), f);
1454 data->idFdX.add(i, i, dfdx);
1455 data->idFdV.add(i, i, dfdv);
1456
1457 if (r_f) {
1458 copy_v3_v3(r_f, f);
1459 }
1460 if (r_dfdx) {
1461 copy_m3_m3(r_dfdx, dfdx);
1462 }
1463 if (r_dfdv) {
1464 copy_m3_m3(r_dfdv, dfdv);
1465 }
1466
1467 return true;
1468 }
1469 else {
1470 if (r_f) {
1471 zero_v3(r_f);
1472 }
1473 if (r_dfdx) {
1474 zero_m3(r_dfdx);
1475 }
1476 if (r_dfdv) {
1477 zero_m3(r_dfdv);
1478 }
1479
1480 return false;
1481 }
1482}
1483
1484#endif /* IMPLICIT_SOLVER_EIGEN */
#define ALMOST_ZERO
Definition BKE_cloth.hh:36
Object is a sort of wrapper for general info.
Read Blender memory allocation and freeing.
#define BLI_INLINE
#define A
btGeneric6DofConstraint & operator=(btGeneric6DofConstraint &other)
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
Definition btQuadWord.h:117
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition btQuadWord.h:119
void reset()
#define printf
#define powf(x, y)
#define index(i)
#define vertex
float length(VecOp< float, D >) RET
ccl_device_inline void copy_v3_v3(ccl_private float *r, const float3 val)
#define M
#define T
#define B
#define F
#define L
void add(Depsgraph &depsgraph, std::function< void()> fn)
MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f)
BLI_INLINE float fbderiv(float length, float L)
BLI_INLINE void print_lvector(const lVector3f &v)
MINLINE void negate_v3_v3(float r[3], const float a[3])
void SIM_mass_spring_clear_forces(struct Implicit_Data *data)
@ SIM_SOLVER_NUMERICAL_ISSUE
@ SIM_SOLVER_NO_CONVERGENCE
@ SIM_SOLVER_INVALID_INPUT
bool SIM_mass_spring_force_spring_goal(struct Implicit_Data *data, int i, const float goal_x[3], const float goal_v[3], float stiffness, float damping)
BLI_INLINE void root_to_world_v3(Implicit_Data *data, int index, float r[3], const float v[3])
BLI_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
DO_INLINE void mul_fvectorT_fvector(float to[3][3], const float vectorA[3], const float vectorB[3])
void SIM_mass_spring_add_constraint_ndof1(struct Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3])
BLI_INLINE float fbstar(float length, float L, float kb, float cb)
void SIM_mass_spring_force_edge_wind(struct Implicit_Data *data, int v1, int v2, float radius1, float radius2, const float(*winvec)[3])
void negate_m3(float R[3][3])
void SIM_mass_spring_add_constraint_ndof0(struct Implicit_Data *data, int index, const float dV[3])
ATTR_WARN_UNUSED_RESULT const BMLoop * l
DO_INLINE void mul_fmatrix_S(float matrix[3][3], float scalar)
void copy_m3_m3(float m1[3][3], const float m2[3][3])
BLI_INLINE void implicit_print_matrix_elem(float v)
Definition implicit.h:43
BLI_INLINE float fb(float length, float L)
BLI_INLINE void apply_spring(Implicit_Data *data, int i, int j, const float f[3], const float dfdx[3][3], const float dfdv[3][3])
Eigen::VectorXf lVector
Definition eigen_utils.h:94
Implicit_Data * SIM_mass_spring_solver_create(int numverts, int numsprings)
BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, float L, float k)
BLI_INLINE void cross_m3_v3m3(float r[3][3], const float v[3], const float m[3][3])
Eigen::SparseMatrix< Scalar > lMatrix
DO_INLINE void sub_fmatrix_fmatrix(float to[3][3], const float matrixA[3][3], const float matrixB[3][3])
void SIM_mass_spring_set_rest_transform(struct Implicit_Data *data, int index, float tfm[3][3])
ATTR_WARN_UNUSED_RESULT const BMVert * v
BLI_INLINE void world_to_root_m3(Implicit_Data *data, int index, float r[3][3], const float m[3][3])
void SIM_mass_spring_force_gravity(struct Implicit_Data *data, int index, float mass, const float g[3])
void SIM_mass_spring_clear_constraints(struct Implicit_Data *data)
void SIM_mass_spring_set_new_velocity(struct Implicit_Data *data, int index, const float v[3])
int SIM_mass_spring_solver_numvert(struct Implicit_Data *id)
MINLINE void mul_v3_fl(float r[3], float f)
BLI_INLINE void dfdv_damp(float to[3][3], const float dir[3], float damping)
MINLINE void sub_v3_v3(float r[3], const float a[3])
void SIM_mass_spring_set_vertex_mass(struct Implicit_Data *data, int index, float mass)
std::vector< Triplet > TripletList
void mul_transposed_m3_v3(const float M[3][3], float r[3])
MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
void SIM_mass_spring_force_extern(struct Implicit_Data *data, int i, const float f[3], float dfdx[3][3], float dfdv[3][3])
void transpose_m3(float R[3][3])
MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
BMesh const char void * data
Eigen::ConjugateGradient< lMatrix, Eigen::Lower, Eigen::DiagonalPreconditioner< Scalar > > ConjugateGradient
BLI_INLINE bool spring_length(Implicit_Data *data, int i, int j, float r_extent[3], float r_dir[3], float *r_length, float r_vel[3])
bool SIM_mass_spring_solve_velocities(struct Implicit_Data *data, float dt, struct ImplicitSolverResult *result)
void SIM_mass_spring_get_new_velocity(struct Implicit_Data *data, int index, float v[3])
void SIM_mass_spring_apply_result(struct Implicit_Data *data)
void SIM_mass_spring_force_drag(struct Implicit_Data *data, float drag)
BLI_INLINE void spring_grad_dir(Implicit_Data *data, int i, int j, float edge[3], float dir[3], float grad_dir[3][3])
BLI_INLINE void world_to_root_v3(Implicit_Data *data, int index, float r[3], const float v[3])
MINLINE float len_v3(const float a[3]) ATTR_WARN_UNUSED_RESULT
void SIM_mass_spring_force_reference_frame(struct Implicit_Data *data, int index, const float acceleration[3], const float omega[3], const float domega_dt[3], float mass)
MINLINE void zero_v3(float r[3])
BLI_INLINE void cross_v3_identity(float r[3][3], const float v[3])
void SIM_mass_spring_get_motion_state(struct Implicit_Data *data, int index, float x[3], float v[3])
bool SIM_mass_spring_force_spring_bending(struct Implicit_Data *data, int i, int j, float restlen, float kb, float cb)
MINLINE float normalize_v3_v3(float r[3], const float a[3])
MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
Eigen::Triplet< Scalar > Triplet
MINLINE float normalize_v3(float n[3])
MINLINE void add_v3_v3(float r[3], const float a[3])
MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
void mul_m3_fl(float R[3][3], float f)
BLI_INLINE void outerproduct(float r[3][3], const float a[3], const float b[3])
void SIM_mass_spring_set_velocity(struct Implicit_Data *data, int index, const float v[3])
void unit_m3(float m[3][3])
void SIM_mass_spring_solver_free(Implicit_Data *id)
BLI_INLINE void root_to_world_m3(Implicit_Data *data, int index, float r[3][3], const float m[3][3])
void SIM_mass_spring_set_position(struct Implicit_Data *data, int index, const float x[3])
void zero_m3(float m[3][3])
void madd_m3_m3m3fl(float R[3][3], const float A[3][3], const float B[3][3], float f)
bool SIM_mass_spring_force_spring_linear(struct Implicit_Data *data, int i, int j, float restlen, float stiffness_tension, float damping_tension, float stiffness_compression, float damping_compression, bool resist_compress, bool new_compress, float clamp_force)
BLI_INLINE void print_lmatrix(const lMatrix &m)
void mul_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
void SIM_mass_spring_force_face_wind(struct Implicit_Data *data, int v1, int v2, int v3, const float(*winvec)[3])
BLI_INLINE void madd_m3_m3fl(float r[3][3], const float m[3][3], float f)
Definition cloth.cc:1261
void mul_v3_m3v3(float r[3], const float M[3][3], const float a[3])
void sub_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
float Scalar
Definition eigen_utils.h:28
MINLINE float dot_v3v3(const float a[3], const float b[3]) ATTR_WARN_UNUSED_RESULT
void SIM_mass_spring_add_constraint_ndof2(struct Implicit_Data *data, int index, const float c1[3], const float dV[3])
void SIM_mass_spring_get_position(struct Implicit_Data *data, int index, float x[3])
void SIM_mass_spring_set_motion_state(struct Implicit_Data *data, int index, const float x[3], const float v[3])
bool SIM_mass_spring_solve_positions(struct Implicit_Data *data, float dt)
void add_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
#define X(TYPE)
#define I
KERNEL_FEATURE_LIGHT_PASSES KERNEL_FEATURE_PATH_GUIDING float
WindowResizeFunc resize
Definition window.cpp:25
CCL_NAMESPACE_BEGIN struct Window V