Program Listing for File PositionBasedElasticRods.cpp
↰ Return to documentation for file (PositionBasedDynamics/PositionBasedElasticRods.cpp)
#include "PositionBasedElasticRods.h"
#include "PositionBasedDynamics/MathFunctions.h"
#include "Utils/Logger.h"
#define _USE_MATH_DEFINES
#include "math.h"
using namespace PBD;
const Real eps = static_cast<Real>(1e-6);
const int permutation[3][3] = {
0, 2, 1,
1, 0, 2,
2, 1, 0
};
// ----------------------------------------------------------------------------------------------
bool PositionBasedCosseratRods::solve_StretchShearConstraint(
const Vector3r& p0, Real invMass0,
const Vector3r& p1, Real invMass1,
const Quaternionr& q0, Real invMassq0,
const Vector3r& stretchingAndShearingKs,
const Real restLength,
Vector3r& corr0, Vector3r& corr1, Quaternionr& corrq0)
{
Vector3r d3; //third director d3 = q0 * e_3 * q0_conjugate
d3[0] = static_cast<Real>(2.0) * (q0.x() * q0.z() + q0.w() * q0.y());
d3[1] = static_cast<Real>(2.0) * (q0.y() * q0.z() - q0.w() * q0.x());
d3[2] = q0.w() * q0.w() - q0.x() * q0.x() - q0.y() * q0.y() + q0.z() * q0.z();
Vector3r gamma = (p1 - p0) / restLength - d3;
gamma /= (invMass1 + invMass0) / restLength + invMassq0 * static_cast<Real>(4.0)*restLength + eps;
if (std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[1]) < eps && std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[2]) < eps) //all Ks are approx. equal
for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i];
else //diffenent stretching and shearing Ks. Transform diag(Ks[0], Ks[1], Ks[2]) into world space using Ks_w = R(q0) * diag(Ks[0], Ks[1], Ks[2]) * R^T(q0) and multiply it with gamma
{
Matrix3r R = q0.toRotationMatrix();
gamma = (R.transpose() * gamma).eval();
for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i];
gamma = (R * gamma).eval();
}
corr0 = invMass0 * gamma;
corr1 = -invMass1 * gamma;
Quaternionr q_e_3_bar(q0.z(), -q0.y(), q0.x(), -q0.w()); //compute q*e_3.conjugate (cheaper than quaternion product)
corrq0 = Quaternionr(0.0, gamma.x(), gamma.y(), gamma.z()) * q_e_3_bar;
corrq0.coeffs() *= static_cast<Real>(2.0) * invMassq0 * restLength;
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedCosseratRods::solve_BendTwistConstraint(
const Quaternionr& q0, Real invMassq0,
const Quaternionr& q1, Real invMassq1,
const Vector3r& bendingAndTwistingKs,
const Quaternionr& restDarbouxVector,
Quaternionr& corrq0, Quaternionr& corrq1)
{
Quaternionr omega = q0.conjugate() * q1; //darboux vector
Quaternionr omega_plus;
omega_plus.coeffs() = omega.coeffs() + restDarbouxVector.coeffs(); //delta Omega with -Omega_0
omega.coeffs() = omega.coeffs() - restDarbouxVector.coeffs(); //delta Omega with + omega_0
if (omega.squaredNorm() > omega_plus.squaredNorm()) omega = omega_plus;
for (int i = 0; i < 3; i++) omega.coeffs()[i] *= bendingAndTwistingKs[i] / (invMassq0 + invMassq1 + static_cast<Real>(1.0e-6));
omega.w() = 0.0; //discrete Darboux vector does not have vanishing scalar part
corrq0 = q1 * omega;
corrq1 = q0 * omega;
corrq0.coeffs() *= invMassq0;
corrq1.coeffs() *= -invMassq1;
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::solve_PerpendiculaBisectorConstraint(
const Vector3r &p0, Real invMass0,
const Vector3r &p1, Real invMass1,
const Vector3r &p2, Real invMass2,
const Real stiffness,
Vector3r &corr0, Vector3r &corr1, Vector3r &corr2)
{
const Vector3r pm = 0.5 * (p0 + p1);
const Vector3r p0p2 = p0 - p2;
const Vector3r p2p1 = p2 - p1;
const Vector3r p1p0 = p1 - p0;
const Vector3r p2pm = p2 - pm;
Real wSum = invMass0 * p0p2.squaredNorm() + invMass1 * p2p1.squaredNorm() + invMass2 * p1p0.squaredNorm();
if (wSum < eps)
return false;
const Real lambda = stiffness * p2pm.dot(p1p0) / wSum;
corr0 = -invMass0 * lambda * p0p2;
corr1 = -invMass1 * lambda * p2p1;
corr2 = -invMass2 * lambda * p1p0;
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::solve_GhostPointEdgeDistanceConstraint(
const Vector3r& p0, Real invMass0,
const Vector3r& p1, Real invMass1,
const Vector3r& p2, Real invMass2,
const Real stiffness,
const Real ghostEdgeRestLength,
Vector3r& corr0, Vector3r& corr1, Vector3r& corr2)
{
// Ghost-Edge constraint
Vector3r pm = 0.5 * (p0 + p1);
Vector3r p2pm = p2 - pm;
Real wSum = static_cast<Real>(0.25) * invMass0 + static_cast<Real>(0.25) * invMass1 + static_cast<Real>(1.0) * invMass2;
if (wSum < eps)
return false;
Real p2pm_mag = p2pm.norm();
p2pm *= static_cast<Real>(1.0) / p2pm_mag;
const Real lambda = stiffness * (p2pm_mag - ghostEdgeRestLength) / wSum;
corr0 = 0.5 * invMass0 * lambda * p2pm;
corr1 = 0.5 * invMass1 * lambda * p2pm;
corr2 = -1.0 * invMass2 * lambda * p2pm;
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::solve_DarbouxVectorConstraint(
const Vector3r& p0, Real invMass0,
const Vector3r& p1, Real invMass1,
const Vector3r& p2, Real invMass2,
const Vector3r& p3, Real invMass3,
const Vector3r& p4, Real invMass4,
const Vector3r& bendingAndTwistingKs,
const Real midEdgeLength,
const Vector3r& restDarbouxVector,
Vector3r& corr0, Vector3r& corr1, Vector3r& corr2, Vector3r& corr3, Vector3r& corr4)
{
// Single rod element:
// 3 4 //ghost points
// | |
// --0---1---2-- // rod points
Vector3r darboux_vector;
Matrix3r d0, d1;
PositionBasedElasticRods::computeMaterialFrame(p0, p1, p3, d0);
PositionBasedElasticRods::computeMaterialFrame(p1, p2, p4, d1);
PositionBasedElasticRods::computeDarbouxVector(d0, d1, midEdgeLength, darboux_vector);
Matrix3r dajpi[3][3];
computeMaterialFrameDerivative(p0, p1, p3, d0,
dajpi[0][0], dajpi[0][1], dajpi[0][2],
dajpi[1][0], dajpi[1][1], dajpi[1][2],
dajpi[2][0], dajpi[2][1], dajpi[2][2]);
Matrix3r dbjpi[3][3];
computeMaterialFrameDerivative(p1, p2, p4, d1,
dbjpi[0][0], dbjpi[0][1], dbjpi[0][2],
dbjpi[1][0], dbjpi[1][1], dbjpi[1][2],
dbjpi[2][0], dbjpi[2][1], dbjpi[2][2]);
Matrix3r constraint_jacobian[5];
computeDarbouxGradient(
darboux_vector, midEdgeLength, d0, d1,
dajpi, dbjpi,
//bendingAndTwistingKs,
constraint_jacobian[0],
constraint_jacobian[1],
constraint_jacobian[2],
constraint_jacobian[3],
constraint_jacobian[4]);
const Vector3r constraint_value(bendingAndTwistingKs[0] * (darboux_vector[0] - restDarbouxVector[0]),
bendingAndTwistingKs[1] * (darboux_vector[1] - restDarbouxVector[1]),
bendingAndTwistingKs[2] * (darboux_vector[2] - restDarbouxVector[2]));
Matrix3r factor_matrix;
factor_matrix.setZero();
Matrix3r tmp_mat;
Real invMasses[]{ invMass0, invMass1, invMass2, invMass3, invMass4 };
for (int i = 0; i < 5; ++i)
{
tmp_mat = constraint_jacobian[i].transpose() * constraint_jacobian[i];
tmp_mat.col(0) *= invMasses[i];
tmp_mat.col(1) *= invMasses[i];
tmp_mat.col(2) *= invMasses[i];
factor_matrix += tmp_mat;
}
Vector3r dp[5];
tmp_mat = factor_matrix.inverse();
for (int i = 0; i < 5; ++i)
{
constraint_jacobian[i].col(0) *= invMasses[i];
constraint_jacobian[i].col(1) *= invMasses[i];
constraint_jacobian[i].col(2) *= invMasses[i];
dp[i] = -(constraint_jacobian[i]) * (tmp_mat * constraint_value);
}
corr0 = dp[0];
corr1 = dp[1];
corr2 = dp[2];
corr3 = dp[3];
corr4 = dp[4];
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::computeMaterialFrame(
const Vector3r& p0,
const Vector3r& p1,
const Vector3r& p2,
Matrix3r& frame)
{
frame.col(2) = (p1 - p0);
frame.col(2).normalize();
frame.col(1) = (frame.col(2).cross(p2 - p0));
frame.col(1).normalize();
frame.col(0) = frame.col(1).cross(frame.col(2));
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::computeDarbouxVector(const Matrix3r& dA, const Matrix3r& dB, const Real mid_edge_length, Vector3r& darboux_vector)
{
Real factor = static_cast<Real>(1.0) + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2));
factor = static_cast<Real>(2.0) / (mid_edge_length * factor);
for (int c = 0; c < 3; ++c)
{
const int i = permutation[c][0];
const int j = permutation[c][1];
const int k = permutation[c][2];
darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j));
}
darboux_vector *= factor;
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::computeMaterialFrameDerivative(
const Vector3r& p0, const Vector3r& p1, const Vector3r& p2, const Matrix3r& d,
Matrix3r& d1p0, Matrix3r& d1p1, Matrix3r& d1p2,
Matrix3r& d2p0, Matrix3r& d2p1, Matrix3r& d2p2,
Matrix3r& d3p0, Matrix3r& d3p1, Matrix3r& d3p2)
{
// d3pi
const Vector3r p01 = p1 - p0;
Real length_p01 = p01.norm();
d3p0.col(0) = d.col(2)[0] * d.col(2);
d3p0.col(1) = d.col(2)[1] * d.col(2);
d3p0.col(2) = d.col(2)[2] * d.col(2);
d3p0.col(0)[0] -= 1.0;
d3p0.col(1)[1] -= 1.0;
d3p0.col(2)[2] -= 1.0;
d3p0.col(0) *= (static_cast<Real>(1.0) / length_p01);
d3p0.col(1) *= (static_cast<Real>(1.0) / length_p01);
d3p0.col(2) *= (static_cast<Real>(1.0) / length_p01);
d3p1.col(0) = -d3p0.col(0);
d3p1.col(1) = -d3p0.col(1);
d3p1.col(2) = -d3p0.col(2);
d3p2.col(0).setZero();
d3p2.col(1).setZero();
d3p2.col(2).setZero();
// d2pi
const Vector3r p02 = p2 - p0;
const Vector3r p01_cross_p02 = p01.cross(p02);
const Real length_cross = p01_cross_p02.norm();
Matrix3r mat;
mat.col(0) = d.col(1)[0] * d.col(1);
mat.col(1) = d.col(1)[1] * d.col(1);
mat.col(2) = d.col(1)[2] * d.col(1);
mat.col(0)[0] -= 1.0;
mat.col(1)[1] -= 1.0;
mat.col(2)[2] -= 1.0;
mat.col(0) *= (-static_cast<Real>(1.0) / length_cross);
mat.col(1) *= (-static_cast<Real>(1.0) / length_cross);
mat.col(2) *= (-static_cast<Real>(1.0) / length_cross);
Matrix3r product_matrix;
MathFunctions::crossProductMatrix(p2 - p1, product_matrix);
d2p0 = mat * product_matrix;
MathFunctions::crossProductMatrix(p0 - p2, product_matrix);
d2p1 = mat * product_matrix;
MathFunctions::crossProductMatrix(p1 - p0, product_matrix);
d2p2 = mat * product_matrix;
// d1pi
Matrix3r product_mat_d3;
Matrix3r product_mat_d2;
MathFunctions::crossProductMatrix(d.col(2), product_mat_d3);
MathFunctions::crossProductMatrix(d.col(1), product_mat_d2);
d1p0 = product_mat_d2 * d3p0 - product_mat_d3 * d2p0;
d1p1 = product_mat_d2 * d3p1 - product_mat_d3 * d2p1;
d1p2 = -product_mat_d3 * d2p2;
return true;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRods::computeDarbouxGradient(
const Vector3r& darboux_vector, const Real length,
const Matrix3r& da, const Matrix3r& db,
const Matrix3r dajpi[3][3], const Matrix3r dbjpi[3][3],
//const Vector3r& bendAndTwistKs,
Matrix3r& omega_pa, Matrix3r& omega_pb, Matrix3r& omega_pc, Matrix3r& omega_pd, Matrix3r& omega_pe
)
{
Real X = static_cast<Real>(1.0) + da.col(0).dot(db.col(0)) + da.col(1).dot(db.col(1)) + da.col(2).dot(db.col(2));
X = static_cast<Real>(2.0) / (length * X);
for (int c = 0; c < 3; ++c)
{
const int i = permutation[c][0];
const int j = permutation[c][1];
const int k = permutation[c][2];
// pa
{
Vector3r term1(0,0,0);
Vector3r term2(0,0,0);
Vector3r tmp(0,0,0);
// first term
term1 = dajpi[j][0].transpose() * db.col(k);
tmp = dajpi[k][0].transpose() * db.col(j);
term1 = term1 - tmp;
// second term
for (int n = 0; n < 3; ++n)
{
tmp = dajpi[n][0].transpose() * db.col(n);
term2 = term2 + tmp;
}
omega_pa.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2);
//omega_pa.col(i) *= bendAndTwistKs[i];
}
// pb
{
Vector3r term1(0, 0, 0);
Vector3r term2(0, 0, 0);
Vector3r tmp(0, 0, 0);
// first term
term1 = dajpi[j][1].transpose() * db.col(k);
tmp = dajpi[k][1].transpose() * db.col(j);
term1 = term1 - tmp;
// third term
tmp = dbjpi[j][0].transpose() * da.col(k);
term1 = term1 - tmp;
tmp = dbjpi[k][0].transpose() * da.col(j);
term1 = term1 + tmp;
// second term
for (int n = 0; n < 3; ++n)
{
tmp = dajpi[n][1].transpose() * db.col(n);
term2 = term2 + tmp;
tmp = dbjpi[n][0].transpose() * da.col(n);
term2 = term2 + tmp;
}
omega_pb.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2);
//omega_pb.col(i) *= bendAndTwistKs[i];
}
// pc
{
Vector3r term1(0, 0, 0);
Vector3r term2(0, 0, 0);
Vector3r tmp(0, 0, 0);
// first term
term1 = dbjpi[j][1].transpose() * da.col(k);
tmp = dbjpi[k][1].transpose() * da.col(j);
term1 = term1 - tmp;
// second term
for (int n = 0; n < 3; ++n)
{
tmp = dbjpi[n][1].transpose() * da.col(n);
term2 = term2 + tmp;
}
omega_pc.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2);
//omega_pc.col(i) *= bendAndTwistKs[i];
}
// pd
{
Vector3r term1(0, 0, 0);
Vector3r term2(0, 0, 0);
Vector3r tmp(0, 0, 0);
// first term
term1 = dajpi[j][2].transpose() * db.col(k);
tmp = dajpi[k][2].transpose() * db.col(j);
term1 = term1 - tmp;
// second term
for (int n = 0; n < 3; ++n) {
tmp = dajpi[n][2].transpose() * db.col(n);
term2 = term2 + tmp;
}
omega_pd.col(i) = X*(term1-(0.5 * darboux_vector[i] * length) * term2);
//omega_pd.col(i) *= bendAndTwistKs[i];
}
// pe
{
Vector3r term1(0, 0, 0);
Vector3r term2(0, 0, 0);
Vector3r tmp(0, 0, 0);
// first term
term1 = dbjpi[j][2].transpose() * da.col(k);
tmp = dbjpi[k][2].transpose() * da.col(j);
term1 -= tmp;
// second term
for (int n = 0; n < 3; ++n)
{
tmp = dbjpi[n][2].transpose() * da.col(n);
term2 += tmp;
}
omega_pe.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2);
//omega_pe.col(i) *= bendAndTwistKs[i];
}
}
return true;
}
// ----------------------------------------------------------------------------------------------
void PBD::DirectPositionBasedSolverForStiffRods::initLists(int numberOfIntervals, std::list <Node*> * &forward, std::list <Node*> * &backward, Node* &root)
{
if (forward != NULL)
delete[] forward;
if (backward != NULL)
delete[] backward;
if (root != NULL)
delete[] root;
forward = new std::list<Node*>[numberOfIntervals];
backward = new std::list<Node*>[numberOfIntervals];
root = new Node[numberOfIntervals];
}
bool PBD::DirectPositionBasedSolverForStiffRods::isSegmentInInterval(RodSegment *segment, int intervalIndex, Interval* intervals, std::vector<RodConstraint*> &rodConstraints, std::vector<RodSegment*> &rodSegments)
{
for (int i = intervals[intervalIndex].start; i <= intervals[intervalIndex].end; i++)
{
if ((segment == rodSegments[rodConstraints[i]->segmentIndex(0)])
|| (segment == rodSegments[rodConstraints[i]->segmentIndex(1)]))
return true;
}
return false;
}
bool PBD::DirectPositionBasedSolverForStiffRods::isConstraintInInterval(RodConstraint *constraint, int intervalIndex, Interval* intervals, std::vector<RodConstraint*> &rodConstraints)
{
for (int i = intervals[intervalIndex].start; i <= intervals[intervalIndex].end; i++)
{
if (constraint == rodConstraints[i])
return true;
}
return false;
}
void PBD::DirectPositionBasedSolverForStiffRods::initSegmentNode(Node *n, int intervalIndex, std::vector<RodConstraint*> &rodConstraints, std::vector<RodSegment*> &rodSegments, std::vector <RodConstraint*> &markedConstraints, Interval* intervals)
{
RodSegment *segment =
(RodSegment*)n->object;
std::vector<RodConstraint*> constraints;
std::vector<int> constraintIndices;
for (int j = 0; j < static_cast<int>(rodConstraints.size()); ++j)
{
RodConstraint* constraint(rodConstraints[j]);
if (rodSegments[constraint->segmentIndex(0)] == segment
|| rodSegments[constraint->segmentIndex(1)] == segment)
{
constraints.push_back(constraint);
constraintIndices.push_back(j);
}
}
for (unsigned int i = 0; i < constraints.size(); i++)
{
if (!isConstraintInInterval(constraints[i], intervalIndex, intervals, rodConstraints))
continue;
// Test whether the edge has been visited before
bool marked = false;
for (unsigned int j = 0; j < markedConstraints.size(); j++)
{
if (constraints[i] == markedConstraints[j])
{
marked = true;
break;
}
}
if (!marked)
{
Node *constraintNode = new Node();
constraintNode->index = constraintIndices[i];
constraintNode->object = constraints[i];
constraintNode->isconstraint = true;
constraintNode->parent = n;
constraintNode->D.setZero();
constraintNode->Dinv.setZero();
constraintNode->J.setZero();
constraintNode->soln.setZero();
n->children.push_back(constraintNode);
Node *segmentNode = new Node();
segmentNode->isconstraint = false;
segmentNode->parent = constraintNode;
// get other segment connected to constraint for new node
if (rodSegments[constraints[i]->segmentIndex(0)] == segment)
{
segmentNode->object = rodSegments[constraints[i]->segmentIndex(1)];
segmentNode->index = constraints[i]->segmentIndex(1);
}
else
{
segmentNode->object = rodSegments[constraints[i]->segmentIndex(0)];
segmentNode->index = constraints[i]->segmentIndex(0);
}
segmentNode->D.setZero();
segmentNode->Dinv.setZero();
segmentNode->J.setZero();
segmentNode->soln.setZero();
constraintNode->children.push_back(segmentNode);
// mark constraint
markedConstraints.push_back(constraints[i]);
initSegmentNode(segmentNode, intervalIndex, rodConstraints,
rodSegments, markedConstraints, intervals);
}
}
}
void PBD::DirectPositionBasedSolverForStiffRods::orderMatrix(Node *n, int intervalIndex, std::list <Node*> * forward, std::list <Node*> * backward)
{
for (unsigned int i = 0; i < n->children.size(); i++)
orderMatrix(n->children[i], intervalIndex, forward, backward);
forward[intervalIndex].push_back(n);
backward[intervalIndex].push_front(n);
}
void PBD::DirectPositionBasedSolverForStiffRods::initNodes(int intervalIndex, std::vector<RodSegment*> &rodSegments, Node* &root, Interval* intervals, std::vector<RodConstraint*> &rodConstraints, std::list <Node*> * forward, std::list <Node*> * backward, std::vector <RodConstraint*> &markedConstraints)
{
// find root
for (int i = 0; i < (int)rodSegments.size(); i++)
{
RodSegment * rb(rodSegments[i]);
if (!isSegmentInInterval(rb, intervalIndex, intervals, rodConstraints, rodSegments))
continue;
else
{
if (root[intervalIndex].object == NULL)
{
root[intervalIndex].object = rb;
root[intervalIndex].index = i;
}
}
if (!rb->isDynamic())
{
root[intervalIndex].object = rb;
root[intervalIndex].index = i;
break;
}
}
root[intervalIndex].isconstraint = false;
root[intervalIndex].parent = NULL;
root[intervalIndex].D.setZero();
root[intervalIndex].Dinv.setZero();
root[intervalIndex].soln.setZero();
initSegmentNode(&root[intervalIndex], intervalIndex, rodConstraints,
rodSegments, markedConstraints, intervals);
orderMatrix(&root[intervalIndex], intervalIndex, forward, backward);
}
void PBD::DirectPositionBasedSolverForStiffRods::initTree(std::vector<RodConstraint*> &rodConstraints, std::vector<RodSegment*> & rodSegments, Interval* &intervals, int &numberOfIntervals, std::list <Node*> * &forward, std::list <Node*> * &backward, Node* &root)
{
numberOfIntervals = 1;
intervals = new Interval[1];
intervals[0].start = 0;
intervals[0].end = (int)rodConstraints.size() - 1;
initLists(numberOfIntervals, forward, backward, root);
std::vector <RodConstraint*> markedConstraints;
for (int i = 0; i < numberOfIntervals; i++)
{
initNodes(i, rodSegments, root, intervals, rodConstraints, forward, backward, markedConstraints);
markedConstraints.clear();
}
}
bool PBD::DirectPositionBasedSolverForStiffRods::computeDarbouxVector(const Quaternionr & q0, const Quaternionr & q1, const Real averageSegmentLength, Vector3r & darbouxVector)
{
darbouxVector = 2. / averageSegmentLength * (q0.conjugate() * q1).vec();
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::computeBendingAndTorsionJacobians(const Quaternionr & q0, const Quaternionr & q1, const Real averageSegmentLength, Eigen::Matrix<Real, 3, 4> & jOmega0, Eigen::Matrix<Real, 3, 4> & jOmega1)
{
jOmega0 <<
-q1.w(), -q1.z(), q1.y(), q1.x(),
q1.z(), -q1.w(), -q1.x(), q1.y(),
-q1.y(), q1.x(), -q1.w(), q1.z();
jOmega1 <<
q0.w(), q0.z(), -q0.y(), -q0.x(),
-q0.z(), q0.w(), q0.x(), -q0.y(),
q0.y(), -q0.x(), q0.w(), -q0.z();
jOmega0 *= static_cast<Real>(2.0) / averageSegmentLength;
jOmega1 *= static_cast<Real>(2.0) / averageSegmentLength;
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::computeMatrixG(const Quaternionr & q, Eigen::Matrix<Real, 4, 3> & G)
{
// w component at index 3
G <<
static_cast<Real>(0.5)*q.w(), static_cast<Real>(0.5)*q.z(), -static_cast<Real>(0.5)*q.y(),
-static_cast<Real>(0.5)*q.z(), static_cast<Real>(0.5)*q.w(), static_cast<Real>(0.5)*q.x(),
static_cast<Real>(0.5)*q.y(), -static_cast<Real>(0.5)*q.x(), static_cast<Real>(0.5)*q.w(),
-static_cast<Real>(0.5)*q.x(), -static_cast<Real>(0.5)*q.y(), -static_cast<Real>(0.5)*q.z();
return true;
}
void PBD::DirectPositionBasedSolverForStiffRods::computeMatrixK(const Vector3r &connector, const Real invMass, const Vector3r &x, const Matrix3r &inertiaInverseW, Matrix3r &K)
{
if (invMass != 0.0)
{
const Vector3r v = connector - x;
const Real a = v[0];
const Real b = v[1];
const Real c = v[2];
// J is symmetric
const Real j11 = inertiaInverseW(0, 0);
const Real j12 = inertiaInverseW(0, 1);
const Real j13 = inertiaInverseW(0, 2);
const Real j22 = inertiaInverseW(1, 1);
const Real j23 = inertiaInverseW(1, 2);
const Real j33 = inertiaInverseW(2, 2);
K(0, 0) = c*c*j22 - b*c*(j23 + j23) + b*b*j33 + invMass;
K(0, 1) = -(c*c*j12) + a*c*j23 + b*c*j13 - a*b*j33;
K(0, 2) = b*c*j12 - a*c*j22 - b*b*j13 + a*b*j23;
K(1, 0) = K(0, 1);
K(1, 1) = c*c*j11 - a*c*(j13 + j13) + a*a*j33 + invMass;
K(1, 2) = -(b*c*j11) + a*c*j12 + a*b*j13 - a*a*j23;
K(2, 0) = K(0, 2);
K(2, 1) = K(1, 2);
K(2, 2) = b*b*j11 - a*b*(j12 + j12) + a*a*j22 + invMass;
}
else
K.setZero();
}
void PBD::DirectPositionBasedSolverForStiffRods::getMassMatrix(RodSegment *segment, Matrix6r &M)
{
if (!segment->isDynamic())
{
M = Matrix6r::Identity();
return;
}
const Vector3r &inertiaLocal = segment->InertiaTensor();
Matrix3r rotationMatrix(segment->Rotation().toRotationMatrix());
Matrix3r inertia(rotationMatrix *
Eigen::DiagonalMatrix<Real, 3>(inertiaLocal) *
rotationMatrix.transpose());
Real mass = segment->Mass();
// Upper half
for (int i = 0; i < 3; i++)
for (int j = 0; j < 6; j++)
if (i == j)
M(i, j) = mass;
else
M(i, j) = 0.0;
// lower left
for (int i = 3; i < 6; i++)
for (int j = 0; j < 3; j++)
M(i, j) = 0.0;
// lower right
for (int i = 3; i < 6; i++)
for (int j = 3; j < 6; j++)
M(i, j) = inertia(i - 3, j - 3);
}
Real PBD::DirectPositionBasedSolverForStiffRods::factor(const int intervalIndex, const std::vector<RodConstraint*> &rodConstraints, std::vector<RodSegment*> & rodSegments, const Interval* &intervals, std::list <Node*> * forward, std::list <Node*> * backward, std::vector<Vector6r> & RHS, std::vector<Vector6r> & lambdaSums, std::vector<std::vector<Matrix3r>> & bendingAndTorsionJacobians)
{
Real maxError(0.);
// compute right hand side of linear equation system
for (size_t currentConstraintIndex = 0; currentConstraintIndex < rodConstraints.size(); ++currentConstraintIndex)
{
RodConstraint* currentConstraint = rodConstraints[currentConstraintIndex];
RodSegment* segment0 = rodSegments[currentConstraint->segmentIndex(0)];
RodSegment* segment1 = rodSegments[currentConstraint->segmentIndex(1)];
const Quaternionr &q0 = segment0->Rotation();
const Quaternionr &q1 = segment1->Rotation();
const Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &constraintInfo(currentConstraint->getConstraintInfo());
Vector6r &rhs(RHS[currentConstraintIndex]);
// Compute zero-stretch part of constraint violation
const Vector3r &connector0 = constraintInfo.col(2);
const Vector3r &connector1 = constraintInfo.col(3);
Vector3r stretchViolation = connector0 - connector1;
// compute Darboux vector (Equation (7))
Vector3r omega;
computeDarbouxVector(q0, q1, currentConstraint->getAverageSegmentLength(), omega);
// Compute bending and torsion part of constraint violation
Vector3r bendingAndTorsionViolation = omega - currentConstraint->getRestDarbouxVector();
// fill right hand side of the linear equation system
const Vector6r &lambdaSum(lambdaSums[currentConstraintIndex]);
rhs.block<3, 1>(0, 0) = -stretchViolation
- Vector3r(currentConstraint->getStretchCompliance().array() *
lambdaSum.block<3, 1>(0, 0).array());
rhs.block<3, 1>(3, 0) = -bendingAndTorsionViolation
- Vector3r(currentConstraint->getBendingAndTorsionCompliance().array() *
lambdaSum.block<3, 1>(3, 0).array());
// compute max error
for (unsigned char i(0); i < 6; ++i)
{
maxError = std::max(maxError, std::abs(rhs[i]));
}
// Compute a part of the Jacobian here, because the relationship
// of the first and second segment to the constraint can be determined directly
// compute G matrices
Eigen::Matrix<Real, 4, 3> G0, G1;
computeMatrixG(q0, G0);
computeMatrixG(q1, G1);
// compute stretching bending Jacobians (Equation (10) and Equation (11))
Eigen::Matrix<Real, 3, 4> jOmega0, jOmega1;
computeBendingAndTorsionJacobians(q0, q1, currentConstraint->getAverageSegmentLength(), jOmega0, jOmega1);
bendingAndTorsionJacobians[currentConstraintIndex][0] = jOmega0*G0;
bendingAndTorsionJacobians[currentConstraintIndex][1] = jOmega1*G1;
}
std::list<Node*>::iterator nodeIter;
for (nodeIter = forward[intervalIndex].begin(); nodeIter != forward[intervalIndex].end(); nodeIter++)
{
Node *node = *nodeIter;
// compute system matrix diagonal
if (node->isconstraint)
{
RodConstraint* currentConstraint = (RodConstraint*)node->object;
//insert compliance
node->D.setZero();
const Vector3r &stretchCompliance(currentConstraint->getStretchCompliance());
node->D(0, 0) -= stretchCompliance[0];
node->D(1, 1) -= stretchCompliance[1];
node->D(2, 2) -= stretchCompliance[2];
const Vector3r &bendingAndTorsionCompliance(currentConstraint->getBendingAndTorsionCompliance());
node->D(3, 3) -= bendingAndTorsionCompliance[0];
node->D(4, 4) -= bendingAndTorsionCompliance[1];
node->D(5, 5) -= bendingAndTorsionCompliance[2];
}
else
{
getMassMatrix((RodSegment*)node->object, node->D);
}
// compute Jacobian
if (node->parent != NULL)
{
if (node->isconstraint)
{
//compute J
RodConstraint *constraint = (RodConstraint*)node->object;
RodSegment *segment = (RodSegment*)node->parent->object;
Real sign = 1;
int segmentIndex = 0;
if (segment == rodSegments[constraint->segmentIndex(1)])
{
segmentIndex = 1;
sign = -1;
}
const Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &constraintInfo(constraint->getConstraintInfo());
const Vector3r r = constraintInfo.col(2 + segmentIndex) - segment->Position();
Matrix3r r_cross;
Real crossSign(-static_cast<Real>(1.0)*sign);
MathFunctions::crossProductMatrix(crossSign*r, r_cross);
Eigen::DiagonalMatrix<Real, 3> upperLeft(sign, sign, sign);
node->J.block<3, 3>(0, 0) = upperLeft;
Matrix3r lowerLeft(Matrix3r::Zero());
node->J.block<3, 3>(3, 0) = lowerLeft;
node->J.block<3, 3>(0, 3) = r_cross;
Matrix3r &lowerRight(bendingAndTorsionJacobians[node->index][segmentIndex]);
node->J.block<3, 3>(3, 3) = lowerRight;
}
else
{
//compute JT
RodConstraint *constraint = (RodConstraint*)node->parent->object;
RodSegment *segment = (RodSegment*)node->object;
Real sign = 1;
int segmentIndex = 0;
if (segment == rodSegments[constraint->segmentIndex(1)])
{
segmentIndex = 1;
sign = -1;
}
const Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &constraintInfo(constraint->getConstraintInfo());
const Vector3r r = constraintInfo.col(2 + segmentIndex) - segment->Position();
Matrix3r r_crossT;
MathFunctions::crossProductMatrix(sign*r, r_crossT);
Eigen::DiagonalMatrix<Real, 3> upperLeft(sign, sign, sign);
node->J.block<3, 3>(0, 0) = upperLeft;
node->J.block<3, 3>(3, 0) = r_crossT;
Matrix3r upperRight(Matrix3r::Zero());
node->J.block<3, 3>(0, 3) = upperRight;
Matrix3r lowerRight(bendingAndTorsionJacobians[node->parent->index][segmentIndex].transpose());
node->J.block<3, 3>(3, 3) = lowerRight;
}
}
}
for (nodeIter = forward[intervalIndex].begin(); nodeIter != forward[intervalIndex].end(); nodeIter++)
{
Node *node = *nodeIter;
std::vector <Node*> children = node->children;
for (size_t i = 0; i < children.size(); i++)
{
Matrix6r JT = (children[i]->J).transpose();
Matrix6r &D = children[i]->D;
Matrix6r &J = children[i]->J;
Matrix6r JTDJ = ((JT * D) * J);
node->D = node->D - JTDJ;
}
bool chk = false;
if (!node->isconstraint)
{
RodSegment *segment = (RodSegment*)node->object;
if (!segment->isDynamic())
{
node->Dinv.setZero();
chk = true;
}
}
node->DLDLT.compute(node->D); // result reused in solve()
if (node->parent != NULL)
{
if (!chk)
{
node->J = node->DLDLT.solve(node->J);
}
else
{
node->J.setZero();
}
}
}
return maxError;
}
bool PBD::DirectPositionBasedSolverForStiffRods::solve(int intervalIndex, std::list <Node*> * forward, std::list <Node*> * backward, std::vector<Vector6r> & RHS, std::vector<Vector6r> & lambdaSums, std::vector<Vector3r> & corr_x, std::vector<Quaternionr> & corr_q)
{
std::list<Node*>::iterator nodeIter;
for (nodeIter = forward[intervalIndex].begin(); nodeIter != forward[intervalIndex].end(); nodeIter++)
{
Node *node = *nodeIter;
if (node->isconstraint)
{
node->soln = -RHS[node->index];
}
else
{
node->soln.setZero();
}
std::vector <Node*> &children = node->children;
for (size_t i = 0; i < children.size(); ++i)
{
Matrix6r cJT = children[i]->J.transpose();
Vector6r &csoln = children[i]->soln;
Vector6r v = cJT * csoln;
node->soln = node->soln - v;
}
}
for (nodeIter = backward[intervalIndex].begin(); nodeIter != backward[intervalIndex].end(); nodeIter++)
{
Node *node = *nodeIter;
bool noZeroDinv(true);
if (!node->isconstraint)
{
RodSegment *segment = (RodSegment*)node->object;
noZeroDinv = segment->isDynamic();
}
if (noZeroDinv) // if DInv == 0 child value is 0 and node->soln is not altered
{
node->soln = node->DLDLT.solve(node->soln);
if (node->parent != NULL)
{
node->soln -= node->J * node->parent->soln;
}
}
else
{
node->soln.setZero(); // segment of node is not dynamic
}
if (node->isconstraint)
{
lambdaSums[node->index] += node->soln;
}
}
// compute position and orientation updates
for (nodeIter = forward[intervalIndex].begin(); nodeIter != forward[intervalIndex].end(); nodeIter++)
{
Node *node = *nodeIter;
if (!node->isconstraint)
{
RodSegment *segment = (RodSegment *)node->object;
if (!segment->isDynamic())
{
break;
}
const Vector6r & soln(node->soln);
Vector3r deltaXSoln = Vector3r(-soln[0], -soln[1], -soln[2]);
corr_x[node->index] = deltaXSoln;
Eigen::Matrix<Real, 4, 3> G;
computeMatrixG(segment->Rotation(), G);
Quaternionr deltaQSoln;
deltaQSoln.coeffs() = G * Vector3r(-soln[3], -soln[4], -soln[5]);
corr_q[node->index] = deltaQSoln;
}
}
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::init_DirectPositionBasedSolverForStiffRodsConstraint(
std::vector<RodConstraint*> &rodConstraints,
std::vector<RodSegment*> & rodSegments,
Interval* &intervals,
int &numberOfIntervals,
std::list <Node*> * &forward,
std::list <Node*> * &backward,
Node* &root,
const std::vector<Vector3r> &constraintPositions,
const std::vector<Real> &averageRadii,
const std::vector<Real> &youngsModuli,
const std::vector<Real> &torsionModuli,
std::vector<Vector6r> & RHS,
std::vector<Vector6r> & lambdaSums,
std::vector<std::vector<Matrix3r>> & bendingAndTorsionJacobians,
std::vector<Vector3r> & corr_x,
std::vector<Quaternionr> & corr_q
)
{
// init constraints
for (size_t cIdx(0); cIdx < rodConstraints.size(); ++cIdx)
{
RodConstraint * constraint(rodConstraints[cIdx]);
RodSegment * segment0(rodSegments[constraint->segmentIndex(0)]);
RodSegment * segment1(rodSegments[constraint->segmentIndex(1)]);
init_StretchBendingTwistingConstraint(
segment0->Position(), segment0->Rotation(), segment1->Position(), segment1->Rotation(),
constraintPositions[cIdx], averageRadii[cIdx], constraint->getAverageSegmentLength(),
youngsModuli[cIdx], torsionModuli[cIdx], constraint->getConstraintInfo(),
constraint->getStiffnessCoefficientK(), constraint->getRestDarbouxVector());
}
// compute tree data structure for direct solver
initTree(rodConstraints, rodSegments, intervals, numberOfIntervals, forward, backward, root);
RHS.resize(rodConstraints.size());
std::fill(RHS.begin(), RHS.end(), Vector6r::Zero());
lambdaSums.resize(rodConstraints.size());
std::fill(lambdaSums.begin(), lambdaSums.end(), Vector6r::Zero());
bendingAndTorsionJacobians.resize(rodConstraints.size());
std::vector<Matrix3r> sampleJacobians(2);
sampleJacobians[0].setZero();
sampleJacobians[1].setZero();
std::fill(bendingAndTorsionJacobians.begin(), bendingAndTorsionJacobians.end(), sampleJacobians);
corr_x.resize(rodSegments.size());
std::fill(corr_x.begin(), corr_x.end(), Vector3r::Zero());
corr_q.resize(rodSegments.size());
std::fill(corr_q.begin(), corr_q.end(), Quaternionr::Identity());
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::initBeforeProjection_DirectPositionBasedSolverForStiffRodsConstraint(
const std::vector<RodConstraint*> &rodConstraints,
const Real inverseTimeStepSize,
std::vector<Vector6r> & lambdaSums
)
{
for (size_t cIdx(0); cIdx < rodConstraints.size(); ++cIdx)
{
RodConstraint * constraint(rodConstraints[cIdx]);
initBeforeProjection_StretchBendingTwistingConstraint(
constraint->getStiffnessCoefficientK(),
inverseTimeStepSize,
constraint->getAverageSegmentLength(),
constraint->getStretchCompliance(),
constraint->getBendingAndTorsionCompliance(),
lambdaSums[cIdx]);
}
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::update_DirectPositionBasedSolverForStiffRodsConstraint(
const std::vector<RodConstraint*> &rodConstraints,
const std::vector<RodSegment*> & rodSegments
)
{
// update rod constraints
for (size_t cIdx(0); cIdx < rodConstraints.size(); ++cIdx)
{
RodConstraint * constraint(rodConstraints[cIdx]);
RodSegment * segment0(rodSegments[constraint->segmentIndex(0)]);
RodSegment * segment1(rodSegments[constraint->segmentIndex(1)]);
update_StretchBendingTwistingConstraint(
segment0->Position(), segment0->Rotation(), segment1->Position(), segment1->Rotation(),
constraint->getConstraintInfo());
}
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::solve_DirectPositionBasedSolverForStiffRodsConstraint(
const std::vector<RodConstraint*> &rodConstraints,
std::vector<RodSegment*> & rodSegments,
const Interval* intervals,
const int &numberOfIntervals,
std::list <Node*> * forward,
std::list <Node*> * backward,
std::vector<Vector6r> & RHS,
std::vector<Vector6r> & lambdaSums,
std::vector<std::vector<Matrix3r>> & bendingAndTorsionJacobians,
std::vector<Vector3r> & corr_x,
std::vector<Quaternionr> & corr_q
)
{
for (int i = 0; i < numberOfIntervals; i++)
{
factor(i, rodConstraints, rodSegments, intervals,
forward, backward, RHS, lambdaSums, bendingAndTorsionJacobians);
}
for (int i = 0; i < numberOfIntervals; i++)
{
solve(i, forward, backward, RHS, lambdaSums, corr_x, corr_q);
}
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::init_StretchBendingTwistingConstraint(
const Vector3r &x0, const Quaternionr &q0,
const Vector3r &x1, const Quaternionr &q1,
const Vector3r &constraintPosition,
const Real averageRadius,
const Real averageSegmentLength,
const Real youngsModulus,
const Real torsionModulus,
Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &constraintInfo,
Vector3r &stiffnessCoefficientK,
Vector3r &restDarbouxVector
)
{
// constraintInfo contains
// 0: connector in segment 0 (local)
// 1: connector in segment 1 (local)
// 2: connector in segment 0 (global)
// 3: connector in segment 1 (global)
// transform in local coordinates
const Matrix3r rot0T = q0.matrix().transpose();
const Matrix3r rot1T = q1.matrix().transpose();
constraintInfo.col(0) = rot0T * (constraintPosition - x0);
constraintInfo.col(1) = rot1T * (constraintPosition - x1);
constraintInfo.col(2) = constraintPosition;
constraintInfo.col(3) = constraintPosition;
// compute bending and torsion stiffness of the K matrix diagonal; assumption: the rod axis follows the y-axis of the local frame as with Blender's armatures
Real secondMomentOfArea(static_cast<Real>(M_PI_4) * std::pow(averageRadius, static_cast<Real>(4.0)));
Real bendingStiffness(youngsModulus * secondMomentOfArea);
Real torsionStiffness(static_cast<Real>(2.0) * torsionModulus * secondMomentOfArea);
stiffnessCoefficientK = Vector3r(bendingStiffness, torsionStiffness, bendingStiffness);
// compute rest Darboux vector
computeDarbouxVector(q0, q1, averageSegmentLength, restDarbouxVector);
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::initBeforeProjection_StretchBendingTwistingConstraint(
const Vector3r &stiffnessCoefficientK,
const Real inverseTimeStepSize,
const Real averageSegmentLength,
Vector3r &stretchCompliance,
Vector3r &bendingAndTorsionCompliance,
Vector6r &lambdaSum
)
{
Real inverseTSQuadratic(inverseTimeStepSize*inverseTimeStepSize);
// compute compliance parameter of the stretch constraint part
const Real stretchRegularizationParameter(static_cast<Real>(1.E-10));
stretchCompliance <<
stretchRegularizationParameter * inverseTSQuadratic,
stretchRegularizationParameter * inverseTSQuadratic,
stretchRegularizationParameter * inverseTSQuadratic;
// compute compliance parameter of the bending and torsion constraint part
bendingAndTorsionCompliance <<
inverseTSQuadratic / stiffnessCoefficientK(0),
inverseTSQuadratic / stiffnessCoefficientK(1),
inverseTSQuadratic / stiffnessCoefficientK(2);
bendingAndTorsionCompliance *= static_cast<Real>(1.0) / averageSegmentLength;
// set sum of delta lambda values to zero
lambdaSum.setZero();
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::update_StretchBendingTwistingConstraint(
const Vector3r &x0, const Quaternionr &q0,
const Vector3r &x1, const Quaternionr &q1,
Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &constraintInfo
)
{
// constraintInfo contains
// 0: connector in segment 0 (local)
// 1: connector in segment 1 (local)
// 2: connector in segment 0 (global)
// 3: connector in segment 1 (global)
// compute world space positions of connectors
const Matrix3r rot0 = q0.matrix();
const Matrix3r rot1 = q1.matrix();
constraintInfo.col(2) = rot0 * constraintInfo.col(0) + x0;
constraintInfo.col(3) = rot1 * constraintInfo.col(1) + x1;
return true;
}
bool PBD::DirectPositionBasedSolverForStiffRods::solve_StretchBendingTwistingConstraint(
const Real invMass0,
const Vector3r &x0,
const Matrix3r &inertiaInverseW0,
const Quaternionr &q0,
const Real invMass1,
const Vector3r &x1,
const Matrix3r &inertiaInverseW1,
const Quaternionr &q1,
const Vector3r &restDarbouxVector,
const Real averageSegmentLength,
const Vector3r &stretchCompliance,
const Vector3r &bendingAndTorsionCompliance,
const Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &constraintInfo,
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1,
Vector6r &lambdaSum
)
{
// compute Darboux vector (Equation (7))
Vector3r omega;
computeDarbouxVector(q0, q1, averageSegmentLength, omega);
// compute bending and torsion Jacobians (Equation (10) and Equation (11))
Eigen::Matrix<Real, 3, 4> jOmega0, jOmega1;
computeBendingAndTorsionJacobians(q0, q1, averageSegmentLength, jOmega0, jOmega1);
// compute G matrices (Equation (27))
Eigen::Matrix<Real, 4, 3> G0, G1;
computeMatrixG(q0, G0);
computeMatrixG(q1, G1);
Matrix3r jOmegaG0(jOmega0*G0),
jOmegaG1(jOmega1*G1);
// Compute zero-stretch part of constraint violation (Equation (23))
const Vector3r &connector0 = constraintInfo.col(2);
const Vector3r &connector1 = constraintInfo.col(3);
Vector3r stretchViolation = connector0 - connector1;
// Compute bending and torsion part of constraint violation (Equation (23))
Vector3r bendingAndTorsionViolation = omega - restDarbouxVector;
// fill right hand side of the linear equation system (Equation (19))
Vector6r rhs;
rhs.block<3, 1>(0, 0) = -stretchViolation
- Vector3r(stretchCompliance.array() * lambdaSum.block<3, 1>(0, 0).array());
rhs.block<3, 1>(3, 0) = -bendingAndTorsionViolation
- Vector3r(bendingAndTorsionCompliance.array() * lambdaSum.block<3, 1>(3, 0).array());
// compute matrix of the linear equation system (using Equations (25), (26), and (28) in Equation (19))
Matrix6r JMJT(Matrix6r::Zero());
// compute stretch block
Matrix3r K1, K2;
computeMatrixK(connector0, invMass0, x0, inertiaInverseW0, K1);
computeMatrixK(connector1, invMass1, x1, inertiaInverseW1, K2);
JMJT.block<3, 3>(0, 0) = K1 + K2;
// compute coupling blocks
const Vector3r ra = connector0 - x0;
const Vector3r rb = connector1 - x1;
Matrix3r ra_crossT, rb_crossT;
MathFunctions::crossProductMatrix(-ra, ra_crossT); // use -ra to get the transpose
MathFunctions::crossProductMatrix(-rb, rb_crossT); // use -rb to get the transpose
Matrix3r offdiag(Matrix3r::Zero());
if (invMass0 != 0.0)
{
offdiag = jOmegaG0 * inertiaInverseW0 * ra_crossT * (-1);
}
if (invMass1 != 0.0)
{
offdiag += jOmegaG1 * inertiaInverseW1 * rb_crossT;
}
JMJT.block<3, 3>(3, 0) = offdiag;
JMJT.block<3, 3>(0, 3) = offdiag.transpose();
// compute bending and torsion block
Matrix3r MInvJT0(inertiaInverseW0 * jOmegaG0.transpose());
Matrix3r MInvJT1(inertiaInverseW1 * jOmegaG1.transpose());
Matrix3r JMJTOmega(Matrix3r::Zero());
if (invMass0 != 0.0)
{
JMJTOmega = jOmegaG0*MInvJT0;
}
if (invMass1 != 0.0)
{
JMJTOmega += jOmegaG1*MInvJT1;
}
JMJT.block<3, 3>(3, 3) = JMJTOmega;
// add compliance
JMJT(0, 0) += stretchCompliance(0);
JMJT(1, 1) += stretchCompliance(1);
JMJT(2, 2) += stretchCompliance(2);
JMJT(3, 3) += bendingAndTorsionCompliance(0);
JMJT(4, 4) += bendingAndTorsionCompliance(1);
JMJT(5, 5) += bendingAndTorsionCompliance(2);
// solve linear equation system (Equation 19)
auto decomposition(JMJT.ldlt());
Vector6r deltaLambda(decomposition.solve(rhs));
// update sum of delta lambda values for next Gauss-Seidel solver iteration step
lambdaSum += deltaLambda;
// compute position and orientation updates (using Equations (25), (26), and (28) in Equation (20))
Vector3r deltaLambdaStretch(deltaLambda.block<3, 1>(0, 0)),
deltaLambdaBendingAndTorsion(deltaLambda.block<3, 1>(3, 0));
corr_x0.setZero();
corr_x1.setZero();
corr_q0.coeffs().setZero();
corr_q1.coeffs().setZero();
if (invMass0 != 0.)
{
corr_x0 += invMass0 * deltaLambdaStretch;
corr_q0.coeffs() += G0 * (inertiaInverseW0 * ra_crossT * (-1 * deltaLambdaStretch) +
MInvJT0 * deltaLambdaBendingAndTorsion);
}
if (invMass1 != 0.)
{
corr_x1 -= invMass1 * deltaLambdaStretch;
corr_q1.coeffs() += G1 * (inertiaInverseW1 * rb_crossT * deltaLambdaStretch +
MInvJT1 * deltaLambdaBendingAndTorsion);
}
return true;
}