@@ -629,8 +629,9 @@ void Elasticity_Kugelstadt2021::initFactorization(std::shared_ptr<Factorization>
629629 unsigned int row_index = 0 ;
630630 for (int i = 0 ; i < (int )numParticles; i++)
631631 {
632- int particleIndex = group[i];
633- const unsigned int i0 = m_current_to_initial_index[particleIndex];
632+ int particleIndex0 = group[i];
633+ const unsigned int i0 = particleIndex0;
634+ unsigned int particleIndex = m_initial_to_current_index[i0];
634635
635636 const double restVolumes_id = m_restVolumes[i];
636637
@@ -651,7 +652,7 @@ void Elasticity_Kugelstadt2021::initFactorization(std::shared_ptr<Factorization>
651652 for (unsigned int j = 0 ; j < numNeighbors; j++)
652653 {
653654 const unsigned int neighborIndex0 = m_initialNeighbors[i0][j];
654- const unsigned int neighborIndex = m_initial_to_current_index[m_initialNeighbors[i0][j] ];
655+ const unsigned int neighborIndex = m_initial_to_current_index[neighborIndex0 ];
655656 const Eigen::Vector3d xj0d = m_model->getPosition0 (neighborIndex0).cast <double >();
656657
657658 const Eigen::Vector3d correctedKernelGradient = m_L[particleIndex].cast <double >() * sim->gradW ((xi0d - xj0d).cast <Real>()).cast <double >();
@@ -680,7 +681,7 @@ void Elasticity_Kugelstadt2021::initFactorization(std::shared_ptr<Factorization>
680681 for (unsigned int k = 0 ; k < numNeighbors; k++)
681682 {
682683 const unsigned int kIndex0 = m_initialNeighbors[i0][k];
683- const unsigned int kIndex = m_initial_to_current_index[m_initialNeighbors[i0][k] ];
684+ const unsigned int kIndex = m_initial_to_current_index[kIndex0 ];
684685 const Eigen::Vector3d xk0d = m_model->getPosition0 (kIndex0 ).cast <double >();
685686
686687 const Eigen::Vector3d correctedKernelGradientd = m_L[particleIndex].cast <double >() * sim->gradW ((xi0d - xk0d).cast <Real>()).cast <double >();
@@ -810,6 +811,7 @@ void Elasticity_Kugelstadt2021::performNeighborhoodSearchSort()
810811 d.sort_field (&m_current_to_initial_index[0 ]);
811812 d.sort_field (&m_L[0 ]);
812813 d.sort_field (&m_restVolumes[0 ]);
814+ d.sort_field (&m_vDiff[0 ]);
813815
814816 for (unsigned int i = 0 ; i < numPart; i++)
815817 m_initial_to_current_index[m_current_to_initial_index[i]] = i;
@@ -845,9 +847,9 @@ void Elasticity_Kugelstadt2021::computeMatrixL()
845847 // ////////////////////////////////////////////////////////////////////////
846848 for (unsigned int j = 0 ; j < numNeighbors; j++)
847849 {
848- const unsigned int neighborIndex = m_initial_to_current_index[m_initialNeighbors[i0][j]];
849850 // get initial neighbor index considering the current particle order
850851 const unsigned int neighborIndex0 = m_initialNeighbors[i0][j];
852+ const unsigned int neighborIndex = m_initial_to_current_index[neighborIndex0];
851853
852854 const Vector3r &xj0 = m_model->getPosition0 (neighborIndex0);
853855 const Vector3r xj_xi_0 = xj0 - xi0;
@@ -1002,7 +1004,7 @@ void Elasticity_Kugelstadt2021::rotationMatricesToAVXQuaternions()
10021004 {
10031005 const int count = std::min (numParticles - i * 8 , 8 );
10041006
1005- // store the deformation gradient of 8 particles in avx vectors
1007+ // store the rotation matrices of 8 particles in avx vectors
10061008 int idx[8 ];
10071009 for (int j = 0 ; j < count; j++)
10081010 idx[j] = m_initial_to_current_index[group[8 * i + j]];
@@ -1011,9 +1013,9 @@ void Elasticity_Kugelstadt2021::rotationMatricesToAVXQuaternions()
10111013
10121014 Quaternionr q[8 ];
10131015 for (auto j = 0 ; j < count; j++)
1014- q[j] = Quaternionr (m_rotations[idx[j]]. transpose () );
1016+ q[j] = Quaternionr (m_rotations[idx[j]]). normalized ( );
10151017 for (auto j = count; j < 8 ; j++)
1016- q[j] = Quaternionr ();
1018+ q[j] = Quaternionr (1 , 0 , 0 , 0 );
10171019
10181020 Quaternion8f& q_avx = quats[i];
10191021 q_avx.set (q);
@@ -1128,9 +1130,9 @@ void Elasticity_Kugelstadt2021::computeRotations()
11281130 R3.store (r2.data ());
11291131 for (auto j = 0 ; j < count; j++)
11301132 {
1131- m_rotations[idx[j]].row (0 ) = r0[j];
1132- m_rotations[idx[j]].row (1 ) = r1[j];
1133- m_rotations[idx[j]].row (2 ) = r2[j];
1133+ m_rotations[idx[j]].col (0 ) = r0[j];
1134+ m_rotations[idx[j]].col (1 ) = r1[j];
1135+ m_rotations[idx[j]].col (2 ) = r2[j];
11341136 }
11351137 }
11361138 }
@@ -1588,9 +1590,9 @@ void Elasticity_Kugelstadt2021::computeRHS(VectorXr & rhs)
15881590 // const Real trace = RTF(0, 0) + RTF(1, 1) + RTF(2, 2) - 3.0;
15891591
15901592 // short form of: trace(R^T F - I)
1591- const Real trace = m_rotations[i].col (0 ).dot (m_F[i].col (0 )) +
1592- m_rotations[i].col (1 ).dot (m_F[i].col (1 )) +
1593- m_rotations[i].col (2 ).dot (m_F[i].col (2 )) - static_cast <Real>(3.0 );
1593+ const Real trace = m_rotations[i].row (0 ).dot (m_F[i].col (0 )) +
1594+ m_rotations[i].row (1 ).dot (m_F[i].col (1 )) +
1595+ m_rotations[i].row (2 ).dot (m_F[i].col (2 )) - static_cast <Real>(3.0 );
15941596
15951597 m_stress[i] = m_lambda * trace;
15961598 }
@@ -2215,7 +2217,7 @@ void Elasticity_Kugelstadt2021::precomputeValues()
22152217 #pragma omp for schedule(static)
22162218 for (int i = 0 ; i < (int )numParticles; i++)
22172219 {
2218- m_RL[i] = m_rotations[i] * m_L[i];
2220+ m_RL[i] = m_rotations[i]. transpose () * m_L[i];
22192221 }
22202222 #pragma omp for schedule(static)
22212223 for (int i = 0 ; i < (int )numParticles; i++)
0 commit comments