1 #include "vec3n.h"
2 //#include "console.h"
3
4 extern int numX;
5
6 //
7 // Cloth - Backward Integrated Spring Network
8 //
9 // (c) Stan Melax 2006
10 // http://www.melax.com/cloth
11 // freeware demo and source
12 // Although its free software, I'll gaurantee and support this software as much as is reasonable.
13 // However, if you choose to use any of this code, then you agree that
14 // I assume no financial liability should the software not meet your expectations.
15 // But do feel free to send any feedback.
16 //
17 // The core backward integration functionality has all been extracted into the SpringNetwork class.
18 // This makes it easy for you if you just want to look at or use the math and the algorithms.
19 // The remainder of the code builds a cloth system with basic render support, I/O, and manipulators,
20 // so its possible to make use of the technology within a 3D application.
21 // This code is separated from the SpringNetwork class in order to avoid pushing a particular style
22 // and prevent any dependancies of the algorithms onto unrelated systems.
23 // Feel free to adapt any of this into your own 3D engine/environment.
24 //
25 // Instead of having unique Hooke force and damping coefficients on each spring, the SpringNetwork
26 // code uses a spring 'type' that indexes a short list of shared named coefficients.
27 // This was just more practical for the typical application of this technology.
28 // Over-designed systems that are too general can be slower for
29 // the next guy to understand and more painful to use.
30 // Editing/creation is easier when only 1 number needs to be changed.
31 // Nonetheless, feel free to adapt to your own needs.
32 //
33
34 #include <stdio.h>
35 #include <float.h>
36
37 #include "vec3n.h"
38 //#include "console.h"
39 //#include "manipulatori.h"
40 //#include "object.h"
41 //#include "xmlparse.h"
42
43 static const float3x3 I(1, 0, 0, 0, 1, 0, 0, 0, 1);
44
dfdx_spring(const float3 & dir,float length,float rest,float k)45 inline float3x3 dfdx_spring(const float3 &dir, float length, float rest, float k)
46 {
47 // dir is unit length direction, rest is spring's restlength, k is spring constant.
48 return ((I - outerprod(dir, dir)) * Min(1.0f, rest / length) - I) * -k;
49 }
dfdx_damp(const float3 & dir,float length,const float3 & vel,float rest,float damping)50 inline float3x3 dfdx_damp(const float3 &dir, float length, const float3 &vel, float rest, float damping)
51 {
52 // inner spring damping vel is the relative velocity of the endpoints.
53 return (I - outerprod(dir, dir)) * (-damping * -(dot(dir, vel) / Max(length, rest)));
54 }
dfdv_damp(const float3 & dir,float damping)55 inline float3x3 dfdv_damp(const float3 &dir, float damping)
56 {
57 // derivative of force wrt velocity.
58 return outerprod(dir, dir) * damping;
59 }
60
61 #include "SpringNetwork.h"
62
SpringNetwork(int _n)63 SpringNetwork::SpringNetwork(int _n) : X(_n), V(_n), F(_n), dV(_n), A(_n), dFdX(_n), dFdV(_n)
64 {
65 assert(SPRING_STRUCT == 0);
66 assert(&spring_shear == &spring_struct + SPRING_SHEAR);
67 assert(&spring_bend == &spring_struct + SPRING_BEND);
68 assert(&spring_struct == &spring_k[SPRING_STRUCT]);
69 assert(&spring_shear == &spring_k[SPRING_SHEAR]);
70 assert(&spring_bend == &spring_k[SPRING_BEND]);
71 // spring_struct=1000000.0f;
72 // spring_shear=1000000.0f;
73
74 spring_struct = 1000.0f;
75 spring_shear = 100.0f;
76
77 spring_bend = 25.0f;
78 spring_damp = 5.0f;
79 spring_air = 1.0f;
80 spring_air = 1.0f;
81 cloth_step = 0.25f; // delta time for cloth
82 cloth_gravity = float3(0, -10, 0);
83 cloth_sleepthreshold = 0.001f;
84 cloth_sleepcount = 100;
85 awake = cloth_sleepcount;
86
87 //fix/pin two points in worldspace
88 float3Nx3N::Block zero;
89 zero.m = float3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
90 zero.c = 0;
91 zero.r = 0;
92 S.blocks.Add(zero);
93 zero.r = numX - 1;
94 S.blocks.Add(zero);
95 }
96
AddBlocks(Spring & s)97 SpringNetwork::Spring &SpringNetwork::AddBlocks(Spring &s)
98 {
99 // Called during initial creation of springs in our spring network.
100 // Sets up the sparse matrices corresponding to connections.
101 // Note the indices (s.iab,s.iba) are also stored with spring to avoid looking them up each time a spring is applied
102 // All 3 matrices A,dFdX, and dFdV are contstructed identically so the block array layout will be the same for each.
103 s.iab = A.blocks.count; // added 'ab' blocks will have this index.
104 A.blocks.Add(float3Nx3N::Block(s.a, s.b));
105 dFdX.blocks.Add(float3Nx3N::Block(s.a, s.b));
106 dFdV.blocks.Add(float3Nx3N::Block(s.a, s.b));
107 s.iba = A.blocks.count; // added 'ba' blocks will have this index.
108 A.blocks.Add(float3Nx3N::Block(s.b, s.a));
109 dFdX.blocks.Add(float3Nx3N::Block(s.b, s.a));
110 dFdV.blocks.Add(float3Nx3N::Block(s.b, s.a));
111 return s;
112 }
113
PreSolveSpring(const SpringNetwork::Spring & s)114 void SpringNetwork::PreSolveSpring(const SpringNetwork::Spring &s)
115 {
116 // Adds this spring's contribution into force vector F and force derivitves dFdX and dFdV
117 // One optimization would be premultiply dfdx by dt*dt and F and dFdV by dt right here in this function.
118 // However, for educational purposes we wont do that now and intead just follow the paper directly.
119 //assert(dFdX.blocks[s.a].c==s.a); // delete this assert, no bugs here
120 //assert(dFdX.blocks[s.a].r==s.a);
121 float3 extent = X[s.b] - X[s.a];
122 float length = magnitude(extent);
123 float3 dir = (length == 0) ? float3(0, 0, 0) : extent * 1.0f / length;
124 float3 vel = V[s.b] - V[s.a];
125 float k = spring_k[s.type];
126 float3 f = dir * ((k * (length - s.restlen)) + spring_damp * dot(vel, dir)); // spring force + damping force
127 F[s.a] += f;
128 F[s.b] -= f;
129 float3x3 dfdx = dfdx_spring(dir, length, s.restlen, k) + dfdx_damp(dir, length, vel, s.restlen, spring_damp);
130 dFdX.blocks[s.a].m -= dfdx; // diagonal chunk dFdX[a,a]
131 dFdX.blocks[s.b].m -= dfdx; // diagonal chunk dFdX[b,b]
132 dFdX.blocks[s.iab].m += dfdx; // off-diag chunk dFdX[a,b]
133 dFdX.blocks[s.iba].m += dfdx; // off-diag chunk dFdX[b,a]
134 float3x3 dfdv = dfdv_damp(dir, spring_damp);
135 dFdV.blocks[s.a].m -= dfdv; // diagonal chunk dFdV[a,a]
136 dFdV.blocks[s.b].m -= dfdv; // diagonal chunk dFdV[b,b]
137 dFdV.blocks[s.iab].m += dfdv; // off-diag chunk dFdV[a,b]
138 dFdV.blocks[s.iba].m += dfdv; // off-diag chunk dFdV[b,a]
139 }
140
CalcForces()141 void SpringNetwork::CalcForces()
142 {
143 // Collect forces and derivatives: F,dFdX,dFdV
144 dFdX.Zero();
145 dFdV.InitDiagonal(-spring_air);
146 F.Init(cloth_gravity);
147
148 F.element[0] = float3(0, 0, 0);
149 F.element[numX - 1] = float3(0, 0, 0);
150
151 F -= V * spring_air;
152 for (int i = 0; i < springs.count; i++)
153 {
154 PreSolveSpring(springs[i]); // will add to F,dFdX,dFdV
155 }
156 }
157
Simulate(float dt)158 void SpringNetwork::Simulate(float dt)
159 {
160 // Get ready for conjugate gradient iterative solver step.
161 // Initialize operands.
162 if (!awake) return;
163 CalcForces();
164 int n = X.count; // all our big vectors are of this size
165 float3N dFdXmV(n); // temp to store result of matrix multiply
166 float3N B(n);
167 dV.Zero();
168 A.Identity(); // build up the big matrix we feed to solver
169 A -= dFdV * dt + dFdX * (dt * dt);
170
171 dFdXmV = dFdX * V;
172 B = F * dt + dFdXmV * (dt * dt);
173
174 ConjGradientFiltered(dV, A, B, S);
175 V = V + dV;
176 // V.element[0] = float3(0,0,0);
177 // V.element[numX-1] = float3(0,0,0);
178
179 X = X + V * dt;
180
181 UpdateLimits();
182 awake = (dot(V, V) < cloth_sleepthreshold) ? awake - 1 : awake = cloth_sleepcount;
183 }
184