Commit 8ed7b835 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

Changes to variable and function names and other clean up.

parent 0e729edd
......@@ -915,12 +915,12 @@ void VegaFemSceneObject::advanceDynamics()
if (offDiagSystemMatrix==nullptr)
offDiagSystemMatrix = new SparseMatrix(*implicitEulerSparse->GetSystemMatrix());
implicitEulerSparse->getEffectiveForce(lcpData->f_eff);
implicitEulerSparse->getEffectiveForce(lcpData->feff);
iterativeConstraintAnticipation();
implicitEulerSparse->assignSolution(lcpData->v_fine_next);
lcpData->v_fine = lcpData->v_fine_next;
implicitEulerSparse->assignSolution(lcpData->velocityAtTdT);
lcpData->velocityAtT = lcpData->velocityAtTdT;
#endif
timestepCounter++;
......@@ -1266,19 +1266,13 @@ int VegaFemSceneObject::getNumFixedDof() const
void VegaFemSceneObject::initializeLcpData(const unsigned int size)
{
std::cout << "here: "<<size;
/*lcpData->temp_1.reserve(size);
lcpData->reactForce.reserve(size);
lcpData->f_eff.reserve(size);
lcpData->v_fine.reserve(size);
lcpData->v_fine_next.reserve(size);
lcpData->v_conv.reserve(size);*/
lcpData->temp_1.resize(size);
lcpData->tempVector.resize(size);
lcpData->reactForce.resize(size);
lcpData->f_eff.resize(size);
lcpData->v_fine.resize(size);
lcpData->v_fine_next.resize(size);
lcpData->v_conv.resize(size);
lcpData->feff.resize(size);
lcpData->velocityAtT.resize(size);
lcpData->velocityAtTdT.resize(size);
lcpData->velocityPrevIteration.resize(size);
lcpData->invBD.resize(9 * getNumNodes());
}
......@@ -1289,17 +1283,14 @@ void VegaFemSceneObject::updateLcpData()
auto contacts = lcpCollisionData->getContacts();
// resize the required arrays to the number of contacts
lcpData->LagMul.resize(contacts.size());
lcpData->LCP_b.resize(contacts.size());
lcpData->lagrangeMultipliers.resize(contacts.size());
lcpData->lcpVector.resize(contacts.size());
lcpData->gapPrev.resize(contacts.size());
lcpData->GnTDAinvGn.resize(contacts.size());
getGap_prev();
lcpData->gnTDAinvGn.resize(contacts.size());
computeGapFromPreviousTimeStep();
implicitEulerSparse->getOffDiagonalMatrix(*offDiagSystemMatrix, lcpData->invBD);
getGnT_DAinv_Gn();
computeGnTDAinvGn();
}
void VegaFemSceneObject::iterativeConstraintAnticipation()
......@@ -1309,105 +1300,100 @@ void VegaFemSceneObject::iterativeConstraintAnticipation()
auto contacts = this->lcpCollisionData->getContacts();
updateLcpData();
std::cout << "iterativeConstraintAnticipation: here 1\n";
if (contacts.size() > 0)
{
updateLcpData();
}
setToConstant(lcpData->v_fine_next, 0.0);
while (cnt < 200)
setToConstant(lcpData->velocityAtTdT, 0.0);
while (cnt < 500)
{
lcpData->v_conv = lcpData->v_fine_next;
lcpData->velocityPrevIteration = lcpData->velocityAtTdT;
if (contacts.size() > 0)
{
/*PART 1: SOLVE THE COMPLEMENTARY PROBLEM USING PROJECTED GAUSS-SEIDEL*/
get_LCP_b();
computeLcpVector();
for (i = 0; i < contacts.size(); i++)
{
this->lcpData->LagMul[i] =
std::max(0.0, (1.0 / lcpData->GnTDAinvGn[i])*-lcpData->LCP_b[i]);
this->lcpData->lagrangeMultipliers[i] =
std::max(0.0, (1.0 / lcpData->gnTDAinvGn[i])*-lcpData->lcpVector[i]);
}
/*PART 2: VELOCITY REFINEMENT USING GAUSS-SEIDEL ITERATION*/
AccumulateForce();
accumulateContactForce();
for (i = 0; i < lcpData->f_eff.size(); i++)
for (i = 0; i < lcpData->feff.size(); i++)
{
lcpData->reactForce[i] += lcpData->f_eff[i];
lcpData->reactForce[i] += lcpData->feff[i];
}
}
else
{
this->lcpData->reactForce = this->lcpData->f_eff;
this->lcpData->reactForce = this->lcpData->feff;
}
for (i = 0; i < 4; i++)
for (i = 0; i < 1; i++)
{
implicitEulerSparse->GetSystemMatrix()->DoOneGaussSeidelIteration(
lcpData->v_fine_next.data(), lcpData->reactForce.data());
lcpData->velocityAtTdT.data(), lcpData->reactForce.data());
}
std::cout << "iterativeConstraintAnticipation: here 3\n";
res = diff2Norm(lcpData->v_fine_next, lcpData->v_conv);
res = diff2Norm(lcpData->velocityAtTdT, lcpData->velocityPrevIteration);
//std::cout << "res: " << res << std::endl;
if (res < 1.0e-8)
if (res < 1.0e-12)
break;
cnt++;
};
std::cout<<cnt<<std::endl;
//std::cout<<cnt<<std::endl;
}
void VegaFemSceneObject::get_LCP_b()
void VegaFemSceneObject::computeLcpVector()
{
int i, dof_id, dof_id2;
int i, dofId1, dofId2;
auto contacts = lcpCollisionData->getContacts();
offDiagSystemMatrix->MultiplyVector(lcpData->v_fine_next.data(), lcpData->temp_1.data());
offDiagSystemMatrix->MultiplyVector(lcpData->velocityAtTdT.data(), lcpData->tempVector.data());
//temp_1 = f_eff - temp_1;
for (i = 0; i < lcpData->feff.size(); i++)
{
lcpData->tempVector[i] = lcpData->feff[i] -lcpData->tempVector[i];
}
for (i = 0; i < contacts.size(); i++)
{
auto c = contacts[i];
dof_id = getDofIdOfNode(contacts.at(i)->index)*3;// starts form 0
dof_id2 = getDofIdOfNode(contacts.at(i)->index);// starts form 0
lcpData->LCP_b[i] =
c->normal(0)*(lcpData->invBD[dof_id] * lcpData->temp_1[dof_id2] + lcpData->invBD[dof_id + 3] * lcpData->temp_1[dof_id2 + 1] + lcpData->invBD[dof_id + 6] * lcpData->temp_1[dof_id2 + 2]) +
c->normal(1)*(lcpData->invBD[dof_id + 1] * lcpData->temp_1[dof_id2] + lcpData->invBD[dof_id + 4] * lcpData->temp_1[dof_id2 + 1] + lcpData->invBD[dof_id + 7] * lcpData->temp_1[dof_id2 + 2]) +
c->normal(2)*(lcpData->invBD[dof_id + 2] * lcpData->temp_1[dof_id2] + lcpData->invBD[dof_id + 5] * lcpData->temp_1[dof_id2 + 1] + lcpData->invBD[dof_id + 8] * lcpData->temp_1[dof_id2 + 2]);
dofId1 = getDofIdOfNode(contacts.at(i)->index) * 3;// starts form 0
dofId2 = getDofIdOfNode(contacts.at(i)->index);// starts form 0
lcpData->LCP_b[i] *= -1;
lcpData->LCP_b[i] =
c->normal(0)*(lcpData->invBD[dof_id] * lcpData->f_eff[dof_id2] + lcpData->invBD[dof_id + 3] * lcpData->f_eff[dof_id2 + 1] + lcpData->invBD[dof_id + 6] * lcpData->f_eff[dof_id2 + 2]) +
c->normal(1)*(lcpData->invBD[dof_id + 1] * lcpData->f_eff[dof_id2] + lcpData->invBD[dof_id + 4] * lcpData->f_eff[dof_id2 + 1] + lcpData->invBD[dof_id + 7] * lcpData->f_eff[dof_id2 + 2]) +
c->normal(2)*(lcpData->invBD[dof_id + 2] * lcpData->f_eff[dof_id2] + lcpData->invBD[dof_id + 5] * lcpData->f_eff[dof_id2 + 1] + lcpData->invBD[dof_id + 8] * lcpData->f_eff[dof_id2 + 2]);
lcpData->lcpVector[i] =
c->normal(0)*(lcpData->invBD[dofId1 ] * lcpData->tempVector[dofId2] + lcpData->invBD[dofId1 + 1] * lcpData->tempVector[dofId2 + 1] + lcpData->invBD[dofId1 + 2] * lcpData->tempVector[dofId2 + 2]) +
c->normal(1)*(lcpData->invBD[dofId1 + 3] * lcpData->tempVector[dofId2] + lcpData->invBD[dofId1 + 4] * lcpData->tempVector[dofId2 + 1] + lcpData->invBD[dofId1 + 5] * lcpData->tempVector[dofId2 + 2]) +
c->normal(2)*(lcpData->invBD[dofId1 + 6] * lcpData->tempVector[dofId2] + lcpData->invBD[dofId1 + 7] * lcpData->tempVector[dofId2 + 1] + lcpData->invBD[dofId1 + 8] * lcpData->tempVector[dofId2 + 2]);
}
//gap at T=t divided by dT
for (i = 0; i < contacts.size(); i++)
{
lcpData->LCP_b[i] += lcpData->gapPrev[i] / implicitEulerSparse->GetTimeStep();
lcpData->lcpVector[i] += lcpData->gapPrev[i] / implicitEulerSparse->GetTimeStep();
}
//rate of change of gap with time at T=t
for (i = 0; i < contacts.size(); i++)
{
auto c = contacts.at(i);
dof_id = getDofIdOfNode(contacts.at(i)->index);// starts form 0
dofId1 = getDofIdOfNode(contacts.at(i)->index);// starts form 0
lcpData->LCP_b[i] +=
(c->normal(0)*lcpData->v_fine[dof_id] +
c->normal(1)*lcpData->v_fine[dof_id + 1] +
c->normal(2)*lcpData->v_fine[dof_id + 2]);
lcpData->lcpVector[i] +=
(c->normal(0)*lcpData->velocityAtT[dofId1 ] +
c->normal(1)*lcpData->velocityAtT[dofId1 + 1] +
c->normal(2)*lcpData->velocityAtT[dofId1 + 2]);
//!! velocity of static object is zero for now
//lcpData->LCP_b[i] -= (c->vel.x*c->normal.x + c->vel.y*c->normal.y + c->vel.z*c->normal.z);
......@@ -1415,54 +1401,131 @@ void VegaFemSceneObject::get_LCP_b()
}
void VegaFemSceneObject::AccumulateForce()
{
//void VegaFemSceneObject::computeLcpVector()
//{
// int i, dofId1, dofId2;
//
// auto contacts = lcpCollisionData->getContacts();
//
// offDiagSystemMatrix->MultiplyVector(lcpData->velocityAtTdT.data(), lcpData->tempVector.data());
//
// for (i = 0; i < contacts.size(); i++)
// {
// auto c = contacts[i];
//
// dofId1 = getDofIdOfNode(contacts.at(i)->index)*3;// starts form 0
// dofId2 = getDofIdOfNode(contacts.at(i)->index);// starts form 0
//
// lcpData->lcpVector[i] =
// c->normal(0)*(lcpData->invBD[dofId1 ] * lcpData->tempVector[dofId2] + lcpData->invBD[dofId1 + 3] * lcpData->tempVector[dofId2 + 1] + lcpData->invBD[dofId1 + 6] * lcpData->tempVector[dofId2 + 2]) +
// c->normal(1)*(lcpData->invBD[dofId1 + 1] * lcpData->tempVector[dofId2] + lcpData->invBD[dofId1 + 4] * lcpData->tempVector[dofId2 + 1] + lcpData->invBD[dofId1 + 7] * lcpData->tempVector[dofId2 + 2]) +
// c->normal(2)*(lcpData->invBD[dofId1 + 2] * lcpData->tempVector[dofId2] + lcpData->invBD[dofId1 + 5] * lcpData->tempVector[dofId2 + 1] + lcpData->invBD[dofId1 + 8] * lcpData->tempVector[dofId2 + 2]);
//
// lcpData->lcpVector[i] *= -1;
//
// lcpData->lcpVector[i] +=
// c->normal(0)*(lcpData->invBD[dofId1 ] * lcpData->feff[dofId2] + lcpData->invBD[dofId1 + 3] * lcpData->feff[dofId2 + 1] + lcpData->invBD[dofId1 + 6] * lcpData->feff[dofId2 + 2]) +
// c->normal(1)*(lcpData->invBD[dofId1 + 1] * lcpData->feff[dofId2] + lcpData->invBD[dofId1 + 4] * lcpData->feff[dofId2 + 1] + lcpData->invBD[dofId1 + 7] * lcpData->feff[dofId2 + 2]) +
// c->normal(2)*(lcpData->invBD[dofId1 + 2] * lcpData->feff[dofId2] + lcpData->invBD[dofId1 + 5] * lcpData->feff[dofId2 + 1] + lcpData->invBD[dofId1 + 8] * lcpData->feff[dofId2 + 2]);
// }
//
// //gap at T=t divided by dT
// for (i = 0; i < contacts.size(); i++)
// {
// lcpData->lcpVector[i] += lcpData->gapPrev[i] / implicitEulerSparse->GetTimeStep();
// }
//
// //rate of change of gap with time at T=t
// for (i = 0; i < contacts.size(); i++)
// {
// auto c = contacts.at(i);
//
// dofId1 = getDofIdOfNode(contacts.at(i)->index);// starts form 0
//
// lcpData->lcpVector[i] +=
// (c->normal(0)*lcpData->velocityAtT[dofId1] +
// c->normal(1)*lcpData->velocityAtT[dofId1 + 1] +
// c->normal(2)*lcpData->velocityAtT[dofId1 + 2]);
//
// //!! velocity of static object is zero for now
// //lcpData->LCP_b[i] -= (c->vel.x*c->normal.x + c->vel.y*c->normal.y + c->vel.z*c->normal.z);
// }
//
//}
int i, dof_id;
double e = 0.4;
void VegaFemSceneObject::accumulateContactForce()
{
int i, dofId;
double e = 4.0e-7;
auto contacts = lcpCollisionData->getContacts();
setToConstant(lcpData->reactForce, 0.0);
for (i = 0; i < contacts.size(); i++)
{
dof_id = getDofIdOfNode(contacts.at(i)->index);// starts form 0
dofId = getDofIdOfNode(contacts.at(i)->index);// starts form 0
auto c = contacts[i];
lcpData->reactForce[dof_id ] = c->normal(0)*lcpData->LagMul[i] * e;
lcpData->reactForce[dof_id + 1] = c->normal(1)*lcpData->LagMul[i] * e;
lcpData->reactForce[dof_id + 2] = c->normal(2)*lcpData->LagMul[i] * e;
lcpData->reactForce[dofId ] = c->normal(0)*lcpData->lagrangeMultipliers[i] * e;
lcpData->reactForce[dofId + 1] = c->normal(1)*lcpData->lagrangeMultipliers[i] * e;
lcpData->reactForce[dofId + 2] = c->normal(2)*lcpData->lagrangeMultipliers[i] * e;
}
}
void VegaFemSceneObject::getGnT_DAinv_Gn()
void VegaFemSceneObject::computeGnTDAinvGn()
{
int dofId;
auto contacts = lcpCollisionData->getContacts();
for (int i = 0; i < contacts.size(); i++)
{
dofId = getDofIdOfNode(contacts.at(i)->index)*3;// starts form 0
dofId = getDofIdOfNode(contacts.at(i)->index) * 3;// starts form 0
auto c = contacts[i];
lcpData->GnTDAinvGn[i] =
lcpData->gnTDAinvGn[i] =
c->normal(0)*
(c->normal(0)*lcpData->invBD[dofId ] +
c->normal(1)*lcpData->invBD[dofId + 3] +
c->normal(2)*lcpData->invBD[dofId + 6]) +
(c->normal(0)*lcpData->invBD[dofId ] +
c->normal(1)*lcpData->invBD[dofId + 1] +
c->normal(2)*lcpData->invBD[dofId + 2]) +
c->normal(1)*
(c->normal(0)*lcpData->invBD[dofId + 1] +
c->normal(1)*lcpData->invBD[dofId + 4] +
c->normal(2)*lcpData->invBD[dofId + 7]) +
(c->normal(0)*lcpData->invBD[dofId + 3] +
c->normal(1)*lcpData->invBD[dofId + 4] +
c->normal(2)*lcpData->invBD[dofId + 5]) +
c->normal(2)*
(c->normal(0)*lcpData->invBD[dofId + 2] +
c->normal(1)*lcpData->invBD[dofId + 5] +
c->normal(2)*lcpData->invBD[dofId + 8]);
(c->normal(0)*lcpData->invBD[dofId + 6] +
c->normal(1)*lcpData->invBD[dofId + 7] +
c->normal(2)*lcpData->invBD[dofId + 8]);
}
}
void VegaFemSceneObject::getGap_prev(){
//void VegaFemSceneObject::computeGnTDAinvGn()
//{
// int dofId;
// auto contacts = lcpCollisionData->getContacts();
//
// for (int i = 0; i < contacts.size(); i++)
// {
// dofId = getDofIdOfNode(contacts.at(i)->index)*3;// starts form 0
// auto c = contacts[i];
//
// lcpData->gnTDAinvGn[i] =
// c->normal(0)*
// (c->normal(0)*lcpData->invBD[dofId ] +
// c->normal(1)*lcpData->invBD[dofId + 3] +
// c->normal(2)*lcpData->invBD[dofId + 6]) +
// c->normal(1)*
// (c->normal(0)*lcpData->invBD[dofId + 1] +
// c->normal(1)*lcpData->invBD[dofId + 4] +
// c->normal(2)*lcpData->invBD[dofId + 7]) +
// c->normal(2)*
// (c->normal(0)*lcpData->invBD[dofId + 2] +
// c->normal(1)*lcpData->invBD[dofId + 5] +
// c->normal(2)*lcpData->invBD[dofId + 8]);
// }
//}
void VegaFemSceneObject::computeGapFromPreviousTimeStep(){
core::Vec3d p;
core::Vec3d planeNormal = core::Vec3d(0.0, 1.0, 0.0);//!!hard-coded
......
......@@ -199,17 +199,17 @@ public:
struct LcpArrays
{
std::vector<double> LagMul;//size = Num. of contacts
std::vector<double> LCP_b;//size = Num. of contacts
std::vector<double> lagrangeMultipliers;//size = Num. of contacts
std::vector<double> lcpVector;//size = Num. of contacts
std::vector<double> gapPrev;//size = Num. of contacts
std::vector<double> GnTDAinvGn;//size = Num. of contacts
std::vector<double> gnTDAinvGn;//size = Num. of contacts
std::vector<double> invBD;//size = 9*Num. of nodes
std::vector<double> temp_1;
std::vector<double> tempVector;
std::vector<double> reactForce;
std::vector<double> f_eff;
std::vector<double> v_fine;
std::vector<double> v_fine_next;
std::vector<double> v_conv;
std::vector<double> feff;
std::vector<double> velocityAtT;
std::vector<double> velocityAtTdT;
std::vector<double> velocityPrevIteration;
};
///
......@@ -241,23 +241,23 @@ public:
///
/// \brief Get the b vector of the LCP equation
///
void get_LCP_b();
void computeLcpVector();
///
/// \brief Accumulates forces based on the Lagrange multipliers
///
void AccumulateForce();
void accumulateContactForce();
///
/// \brief Get the M matrix of the LCP
/// inputs normals, invBD, list of contacts, ID
///
void getGnT_DAinv_Gn();
void computeGnTDAinvGn();
///
/// \brief Get the gap from previous time step of all the nodes that are on contact
///
void getGap_prev();
void computeGapFromPreviousTimeStep();
///
/// \brief Accumulates forces based on the Lagrange multipliers
......
......@@ -415,41 +415,34 @@ int ImplicitBackwardEulerSparseLcp::getColIndex(int row, int col)
std::cout << "Error -1:" << row<< "\t" <<col<< "\n";
return -1;
}
void ImplicitBackwardEulerSparseLcp::getOffDiagonalMatrix(SparseMatrix& m,
std::vector<double>& BDarray)//checked
std::vector<double>& BDarray)
{
int j;
Eigen::Matrix3d aii;
std::cout << "getOffDiagonalMatrix: here 0\n";
m = *systemMatrix;
int size = m.Getn();
std::cout << "getOffDiagonalMatrix: here 1\n";
for (int i = 0; i < size; i++)
{
//std::cout << "i:"<<i<<"\n";
if (i % 3 == 0)
{
m.AddEntry(i, i, -systemMatrix->GetEntry(i, getColIndex(i,i)));
m.AddEntry(i, i + 1, -systemMatrix->GetEntry(i, getColIndex(i, i+1)));
m.AddEntry(i, i + 2, -systemMatrix->GetEntry(i, getColIndex(i, i+2)));
/*aii(0, 0) = systemMatrix->GetEntry(i, i);
aii(0, 1) = systemMatrix->GetEntry(i, i + 1);
aii(0, 2) = systemMatrix->GetEntry(i, i + 2);
// compute and store diagonal block in a linear array
aii(0, 0) = systemMatrix->GetEntry(i, getColIndex(i, i));
aii(0, 1) = systemMatrix->GetEntry(i, getColIndex(i, i+1));
aii(0, 2) = systemMatrix->GetEntry(i, getColIndex(i, i+2));
aii(1, 0) = systemMatrix->GetEntry(i+1, i);
aii(1, 1) = systemMatrix->GetEntry(i+1, i + 1);
aii(1, 2) = systemMatrix->GetEntry(i+1, i + 2);
aii(1, 0) = systemMatrix->GetEntry(i + 1, getColIndex(i+1, i));
aii(1, 1) = systemMatrix->GetEntry(i + 1, getColIndex(i+1, i+1));
aii(1, 2) = systemMatrix->GetEntry(i + 1, getColIndex(i+2, i+2));
aii(2, 0) = systemMatrix->GetEntry(i+2, i);
aii(2, 1) = systemMatrix->GetEntry(i+2, i + 1);
aii(2, 2) = systemMatrix->GetEntry(i+2, i + 2);
aii(2, 0) = systemMatrix->GetEntry(i + 2, getColIndex(i+2, i));
aii(2, 1) = systemMatrix->GetEntry(i + 2, getColIndex(i+2, i+1));
aii(2, 2) = systemMatrix->GetEntry(i + 2, getColIndex(i+2, i+2));
aii = aii.inverse();
......@@ -465,26 +458,21 @@ void ImplicitBackwardEulerSparseLcp::getOffDiagonalMatrix(SparseMatrix& m,
BDarray.at(j + 6) = aii(2, 0);
BDarray.at(j + 7) = aii(2, 1);
BDarray.at(j + 8) = aii(2, 2);*/
BDarray.at(j + 8) = aii(2, 2);
// remove diagonal block
/*m.SetEntry(i, getColIndex(i, i), 0.0);
m.SetEntry(i, getColIndex(i, i + 1), 0.0);
m.SetEntry(i, getColIndex(i, i + 2), 0.0);
m.SetEntry(i + 1, getColIndex(i + 1, i), 0.0);
m.SetEntry(i + 1, getColIndex(i + 1, i + 1), 0.0);
m.SetEntry(i + 1, getColIndex(i + 1, i + 2), 0.0);
m.SetEntry(i + 2, getColIndex(i + 2, i), 0.0);
m.SetEntry(i + 2, getColIndex(i + 2, i + 1), 0.0);
m.SetEntry(i + 2, getColIndex(i + 2, i + 2), 0.0);*/
}
else if (i % 3 == 1)
{
m.AddEntry(i, i, -systemMatrix->GetEntry(i, getColIndex(i, i)));
m.AddEntry(i, i - 1, -systemMatrix->GetEntry(i, getColIndex(i, i-1)));
m.AddEntry(i, i + 1, -systemMatrix->GetEntry(i, getColIndex(i, i+1)));
}
else if (i % 3 == 2)
{
m.AddEntry(i, i, -systemMatrix->GetEntry(i, getColIndex(i, i)));
m.AddEntry(i, i - 1, -systemMatrix->GetEntry(i, getColIndex(i, i-1)));
m.AddEntry(i, i - 2, -systemMatrix->GetEntry(i, getColIndex(i, i-2)));
}
else
{
std::cout << "Error: size not in multiples of 3!\n";
}
}
std::cout << "getOffDiagonalMatrix: here 2\n";
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment