Skip to content

Commit 1b0a564

Browse files
author
bender
committed
- added shape matching to bar demo
- added more stable polar decomposition
1 parent aa7e1ab commit 1b0a564

File tree

9 files changed

+188
-29
lines changed

9 files changed

+188
-29
lines changed

‎Demos/BarDemo/TetModel.cpp‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,6 @@ void TetModel::setGeometry(const unsigned int nPoints, Eigen::Vector3f* coords,
5050
m_particleMesh.buildNeighbors();
5151

5252
createVisMesh();
53-
54-
initConstraints();
5553
}
5654

5755
void TetModel::reset()
@@ -86,10 +84,18 @@ void TetModel::initTetConstraints()
8684
const Eigen::Vector3f &x2 = pd.getPosition(tets[4 * i + 1]);
8785
const Eigen::Vector3f &x3 = pd.getPosition(tets[4 * i + 2]);
8886
const Eigen::Vector3f &x4 = pd.getPosition(tets[4 * i + 3]);
87+
Eigen::Vector3f x[4] = { x1, x2, x3, x4 };
88+
float w[4] = {
89+
pd.getInvMass(tets[4 * i]),
90+
pd.getInvMass(tets[4 * i + 1]),
91+
pd.getInvMass(tets[4 * i + 2]) ,
92+
pd.getInvMass(tets[4 * i + 3])
93+
};
8994

9095
TetConstraint tc;
9196
PositionBasedDynamics::computeStrainTetraInvRestMat(x1, x2, x3, x4, tc.invRestMat_SBD);
9297
PositionBasedDynamics::computeFEMTetraInvRestMat(x1, x2, x3, x4, tc.tetVolume, tc.invRestMat_FEM);
98+
PositionBasedDynamics::computeShapeMatchingRestInfo(x, w, 4, tc.restCm_SM, tc.invRestMat_SM);
9399
m_tetConstraints.push_back(tc);
94100
}
95101
}

‎Demos/BarDemo/TetModel.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,11 +17,15 @@ namespace PBD
1717
float tetVolume;
1818
Eigen::Matrix3f invRestMat_SBD;
1919
Eigen::Matrix3f invRestMat_FEM;
20+
Eigen::Vector3f restCm_SM;
21+
Eigen::Matrix3f invRestMat_SM;
2022
};
2123

2224
TetModel();
2325
virtual ~TetModel();
2426

27+
void initConstraints();
28+
2529
typedef IndexedFaceMesh<VertexData> VisMesh;
2630
typedef IndexedTetMesh<ParticleData> ParticleMesh;
2731

@@ -34,8 +38,7 @@ namespace PBD
3438
float m_poissonRatio;
3539
bool m_normalizeStretch;
3640
bool m_normalizeShear;
37-
38-
void initConstraints();
41+
3942
void initTetConstraints();
4043
void createVisMesh();
4144

‎Demos/BarDemo/TimeStepTetModel.cpp‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@ void TimeStepTetModel::constraintProjection(TetModel &model)
8686

8787
ParticleData &pd = model.getParticleMesh().getVertexData();
8888
std::vector<TetModel::TetConstraint> &tetConstraints = model.getTetConstraints();
89+
const IndexedTetMesh<ParticleData>::VertexTets *vTets = model.getParticleMesh().getVertexTets().data();
8990

9091
while (iter < maxIter)
9192
{
@@ -188,6 +189,33 @@ void TimeStepTetModel::constraintProjection(TetModel &model)
188189
model.getNormalizeShear(),
189190
corr1, corr2, corr3, corr4);
190191
}
192+
else if (m_simulationMethod == 4) // shape matching
193+
{
194+
Eigen::Vector3f &x1_0 = pd.getPosition0(v1);
195+
Eigen::Vector3f &x2_0 = pd.getPosition0(v2);
196+
Eigen::Vector3f &x3_0 = pd.getPosition0(v3);
197+
Eigen::Vector3f &x4_0 = pd.getPosition0(v4);
198+
199+
Eigen::Vector3f x[4] = { x1, x2, x3, x4 };
200+
Eigen::Vector3f x0[4] = { x1_0, x2_0, x3_0, x4_0 };
201+
float w[4] = { invMass1, invMass2, invMass3, invMass4 };
202+
203+
Eigen::Vector3f corr[4];
204+
res = PositionBasedDynamics::solveShapeMatchingConstraint(
205+
x0, x, w, 4,
206+
tetConstraints[i].restCm_SM,
207+
tetConstraints[i].invRestMat_SM,
208+
model.getStiffness(),
209+
false,
210+
corr);
211+
212+
// Important: Divide position correction by the number of clusters
213+
// which contain the vertex.
214+
corr1 = (1.0f / vTets[v1].m_numTets) * corr[0];
215+
corr2 = (1.0f / vTets[v2].m_numTets) * corr[1];
216+
corr3 = (1.0f / vTets[v3].m_numTets) * corr[2];
217+
corr4 = (1.0f / vTets[v4].m_numTets) * corr[3];
218+
}
191219

192220
if (res)
193221
{

‎Demos/BarDemo/main.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,8 @@ int main( int argc, char **argv )
6868
TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &pause, " label='Pause' group=Simulation key=SPACE ");
6969
TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_FLOAT, setTimeStep, getTimeStep, &model, " label='Time step size' min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
7070
TwType enumType = TwDefineEnum("SimulationMethodType", NULL, 0);
71-
TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType, setSimulationMethod, getSimulationMethod, &simulation, " label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}' group=Simulation");
71+
TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType, setSimulationMethod, getSimulationMethod, &simulation,
72+
" label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}' group=Simulation");
7273
TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_FLOAT, setStiffness, getStiffness, &model, " label='Stiffness' min=0.0 step=0.1 precision=4 group='Simulation' ");
7374
TwAddVarCB(MiniGL::getTweakBar(), "PoissonRatio", TW_TYPE_FLOAT, setPoissonRatio, getPoissonRatio, &model, " label='Poisson ratio XY' min=0.0 step=0.1 precision=4 group='Simulation' ");
7475
TwAddVarCB(MiniGL::getTweakBar(), "NormalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, &model, " label='Normalize stretch' group='Strain based dynamics' ");
@@ -263,6 +264,7 @@ void createMesh()
263264
pd.setMass(i*height*depth + j*depth + k, 0.0);
264265
}
265266
}
267+
model.initConstraints();
266268

267269
std::cout << "Number of tets: " << indices.size() / 4 << "\n";
268270
std::cout << "Number of vertices: " << width*height*depth << "\n";

‎Demos/ClothDemo/TriangleModel.cpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,6 @@ void TriangleModel::setGeometry(const unsigned int nPoints, Eigen::Vector3f* coo
5959
// Update normals
6060
m_particleMesh.updateNormals();
6161
m_particleMesh.updateVertexNormals();
62-
63-
initConstraints();
6462
}
6563

6664

‎Demos/ClothDemo/TriangleModel.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -47,15 +47,16 @@ namespace PBD
4747
float m_yxPoissonRatio;
4848
bool m_normalizeStretch;
4949
bool m_normalizeShear;
50-
51-
void initConstraints();
50+
5251
void initBendingConstraints();
5352
void initTriangleConstraints();
5453

5554
public:
5655
ParticleMesh &getParticleMesh();
5756
void cleanupModel();
5857

58+
void initConstraints();
59+
5960
float getStiffness() const { return m_stiffness; }
6061
void setStiffness(float val) { m_stiffness = val; }
6162
float getBendingStiffness() const { return m_bendingStiffness; }

‎Demos/ClothDemo/main.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <Eigen/Dense>
77
#include "TriangleModel.h"
88
#include "TimeStepTriangleModel.h"
9+
#include <iostream>
910

1011
// Enable memory leak detection
1112
#ifdef _DEBUG
@@ -281,6 +282,12 @@ void createMesh()
281282
// Set mass of points to zero => make it static
282283
pd.setMass(0, 0.0);
283284
pd.setMass((nRows-1)*nCols, 0.0);
285+
286+
model.initConstraints();
287+
288+
std::cout << "Number of triangles: " << nIndices / 3 << "\n";
289+
std::cout << "Number of vertices: " << nRows*nCols << "\n";
290+
284291
}
285292

286293
void TW_CALL setTimeStep(const void *value, void *clientData)

‎PositionBasedDynamics/PositionBasedDynamics.cpp‎

Lines changed: 120 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -140,6 +140,114 @@ static void polarDecomposition(const Eigen::Matrix3f &A, Eigen::Matrix3f &R, Eig
140140
R.col(2) = c2;
141141
}
142142

143+
/** Return the one norm of the matrix.
144+
*/
145+
static float oneNorm(const Eigen::Matrix3f &A)
146+
{
147+
const float sum1 = fabs(A(0,0)) + fabs(A(1,0)) + fabs(A(2,0));
148+
const float sum2 = fabs(A(0,1)) + fabs(A(1,1)) + fabs(A(2,1));
149+
const float sum3 = fabs(A(0,2)) + fabs(A(1,2)) + fabs(A(2,2));
150+
float maxSum = sum1;
151+
if (sum2 > maxSum)
152+
maxSum = sum2;
153+
if (sum3 > maxSum)
154+
maxSum = sum3;
155+
return maxSum;
156+
}
157+
158+
/** Return the inf norm of the matrix.
159+
*/
160+
static float infNorm(const Eigen::Matrix3f &A)
161+
{
162+
const float sum1 = fabs(A(0, 0)) + fabs(A(0, 1)) + fabs(A(0, 2));
163+
const float sum2 = fabs(A(1, 0)) + fabs(A(1, 1)) + fabs(A(1, 2));
164+
const float sum3 = fabs(A(2, 0)) + fabs(A(2, 1)) + fabs(A(2, 2));
165+
float maxSum = sum1;
166+
if (sum2 > maxSum)
167+
maxSum = sum2;
168+
if (sum3 > maxSum)
169+
maxSum = sum3;
170+
return maxSum;
171+
}
172+
173+
/** Perform a polar decomposition of matrix M and return the rotation matrix R. This method handles the degenerated cases.
174+
*/
175+
static void polarDecomposition2(const Eigen::Matrix3f &M, const float tolerance, Eigen::Matrix3f &R)
176+
{
177+
Eigen::Matrix3f Mt = M.transpose();
178+
float Mone = oneNorm(M);
179+
float Minf = infNorm(M);
180+
float Eone;
181+
Eigen::Matrix3f MadjTt, Et;
182+
do
183+
{
184+
MadjTt.row(0) = Mt.row(1).cross(Mt.row(2));
185+
MadjTt.row(1) = Mt.row(2).cross(Mt.row(0));
186+
MadjTt.row(2) = Mt.row(0).cross(Mt.row(1));
187+
188+
float det = Mt(0,0) * MadjTt(0,0) + Mt(0,1) * MadjTt(0,1) + Mt(0,2) * MadjTt(0,2);
189+
190+
if (fabs(det) < 1.0e-12)
191+
{
192+
Eigen::Vector3f len;
193+
unsigned int index = 0xffffffff;
194+
for (unsigned int i = 0; i < 3; i++)
195+
{
196+
len[i] = MadjTt.row(i).squaredNorm();
197+
if (len[i] > 1.0e-12)
198+
{
199+
// index of valid cross product
200+
// => is also the index of the vector in Mt that must be exchanged
201+
index = i;
202+
break;
203+
}
204+
}
205+
if (index == 0xffffffff)
206+
{
207+
R.setIdentity();
208+
return;
209+
}
210+
else
211+
{
212+
Mt.row(index) = Mt.row((index + 1) % 3).cross(Mt.row((index + 2) % 3));
213+
MadjTt.row((index + 1) % 3) = Mt.row((index + 2) % 3).cross(Mt.row(index));
214+
MadjTt.row((index + 2) % 3) = Mt.row(index).cross(Mt.row((index + 1) % 3));
215+
Eigen::Matrix3f M2 = Mt.transpose();
216+
Mone = oneNorm(M2);
217+
Minf = infNorm(M2);
218+
det = Mt(0,0) * MadjTt(0,0) + Mt(0,1) * MadjTt(0,1) + Mt(0,2) * MadjTt(0,2);
219+
}
220+
}
221+
222+
const float MadjTone = oneNorm(MadjTt);
223+
const float MadjTinf = infNorm(MadjTt);
224+
225+
const float gamma = sqrt(sqrt((MadjTone*MadjTinf) / (Mone*Minf)) / fabs(det));
226+
227+
const float g1 = gamma*0.5f;
228+
const float g2 = 0.5f / (gamma*det);
229+
230+
for (unsigned char i = 0; i < 3; i++)
231+
{
232+
for (unsigned char j = 0; j < 3; j++)
233+
{
234+
Et(i,j) = Mt(i,j);
235+
Mt(i,j) = g1*Mt(i,j) + g2*MadjTt(i,j);
236+
Et(i,j) -= Mt(i,j);
237+
}
238+
}
239+
240+
Eone = oneNorm(Et);
241+
242+
Mone = oneNorm(Mt);
243+
Minf = infNorm(Mt);
244+
} while (Eone > Mone * tolerance);
245+
246+
// Q = Mt^T
247+
R = Mt.transpose();
248+
}
249+
250+
143251
// ----------------------------------------------------------------------------------------------
144252
bool PositionBasedDynamics::solveDistanceConstraint(
145253
const Eigen::Vector3f &p0, float invMass0,
@@ -653,7 +761,8 @@ bool PositionBasedDynamics::computeShapeMatchingRestInfo(
653761
restCm /= wsum;
654762

655763
// A
656-
invRestMat.setZero();
764+
Eigen::Matrix3f A;
765+
A.setZero();
657766
for (int i = 0; i < numPoints; i++) {
658767
const Eigen::Vector3f qi = x0[i] - restCm;
659768
float wi = 1.0f / (invMasses[i] + eps);
@@ -663,21 +772,21 @@ bool PositionBasedDynamics::computeShapeMatchingRestInfo(
663772
float xy = wi * qi[0] * qi[1];
664773
float xz = wi * qi[0] * qi[2];
665774
float yz = wi * qi[1] * qi[2];
666-
invRestMat(0, 0) += x2; invRestMat(0, 1) += xy; invRestMat(0, 2) += xz;
667-
invRestMat(1, 0) += xy; invRestMat(1, 1) += y2; invRestMat(1, 2) += yz;
668-
invRestMat(2, 0) += xz; invRestMat(2, 1) += yz; invRestMat(2, 2) += z2;
775+
A(0, 0) += x2; A(0, 1) += xy; A(0, 2) += xz;
776+
A(1, 0) += xy; A(1, 1) += y2; A(1, 2) += yz;
777+
A(2, 0) += xz; A(2, 1) += yz; A(2, 2) += z2;
669778
}
670-
float det = invRestMat.determinant();
779+
float det = A.determinant();
671780
if (fabs(det) > 1.0e-9)
672781
{
673-
invRestMat = invRestMat.inverse();
782+
invRestMat = A.inverse();
674783
return true;
675784
}
676785
return false;
677786
}
678787

679788
// ----------------------------------------------------------------------------------------------
680-
bool PositionBasedDynamics::shapeMatchingConstraint(
789+
bool PositionBasedDynamics::solveShapeMatchingConstraint(
681790
const Eigen::Vector3f x0[], const Eigen::Vector3f x[], const float invMasses[], int numPoints,
682791
Eigen::Vector3f &restCm, const Eigen::Matrix3f &invRestMat,
683792
float stiffness,
@@ -692,7 +801,8 @@ bool PositionBasedDynamics::shapeMatchingConstraint(
692801
// center of mass
693802
Eigen::Vector3f cm(0.0f, 0.0f, 0.0f);
694803
float wsum = 0.0f;
695-
for (int i = 0; i < numPoints; i++) {
804+
for (int i = 0; i < numPoints; i++)
805+
{
696806
float wi = 1.0f / (invMasses[i] + eps);
697807
cm += x[i] * wi;
698808
wsum += wi;
@@ -723,7 +833,8 @@ bool PositionBasedDynamics::shapeMatchingConstraint(
723833
if (allowStretch)
724834
R = mat;
725835
else
726-
polarDecomposition(mat, R, U, D);
836+
//polarDecomposition(mat, R, U, D);
837+
polarDecomposition2(mat, 1e-6f, R);
727838

728839
for (int i = 0; i < numPoints; i++) {
729840
Eigen::Vector3f goal = cm + R * (x0[i] - restCm);

‎PositionBasedDynamics/PositionBasedDynamics.h‎

Lines changed: 14 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -40,17 +40,6 @@ namespace PBD
4040
float posVolumeStiffness,
4141
Eigen::Vector3f &corr0, Eigen::Vector3f &corr1, Eigen::Vector3f &corr2, Eigen::Vector3f &corr3);
4242

43-
static bool computeShapeMatchingRestInfo(
44-
const Eigen::Vector3f x0[], const float invMasses[], int numPoints,
45-
Eigen::Vector3f &restCm, Eigen::Matrix3f &invRestMat);
46-
47-
static bool shapeMatchingConstraint(
48-
const Eigen::Vector3f x0[], const Eigen::Vector3f x[], const float invMasses[], int numPoints,
49-
Eigen::Vector3f &restCm, const Eigen::Matrix3f &invRestMat,
50-
float stiffness,
51-
bool allowStretch, // default false
52-
Eigen::Vector3f corr[], Eigen::Matrix3f *rot = NULL);
53-
5443
static bool solveEdgePointDistConstraint(
5544
const Eigen::Vector3f &p, float invMass,
5645
const Eigen::Vector3f &p0, float invMass0,
@@ -102,6 +91,20 @@ namespace PBD
10291
float stiffness,
10392
Eigen::Vector3f &corr0, Eigen::Vector3f &corr1, Eigen::Vector3f &corr2, Eigen::Vector3f &corr3);
10493

94+
// -------------- Shape Matching -----------------------------------------------------
95+
96+
static bool computeShapeMatchingRestInfo(
97+
const Eigen::Vector3f x0[], const float invMasses[], const int numPoints,
98+
Eigen::Vector3f &restCm, Eigen::Matrix3f &invRestMat);
99+
100+
static bool solveShapeMatchingConstraint(
101+
const Eigen::Vector3f x0[], const Eigen::Vector3f x[], const float invMasses[], const int numPoints,
102+
Eigen::Vector3f &restCm, const Eigen::Matrix3f &invRestMat,
103+
float stiffness,
104+
bool allowStretch, // default false
105+
Eigen::Vector3f corr[], Eigen::Matrix3f *rot = NULL);
106+
107+
105108
// -------------- Strain Based Dynamics -----------------------------------------------------
106109

107110
static bool computeStrainTriangleInvRestMat( // compute only when rest shape changes, triangle in the xy plane

0 commit comments

Comments
 (0)