manolas
/
MySources
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
MySources/der_leimer.cpp

4846 lines
304 KiB
C++
Raw Blame History

#include "der_leimer.hpp"
#include <unordered_set>
#define MAT(mat, m, row, col) mat[m * col + row]
//#define EXPLICIT_BC
DER_leimer::DER_leimer() {}
Eigen::Vector3d perpToVector(const Eigen::Vector3d &v)
{
Eigen::Vector3d result;
int mincoord = 0;
double minval = std::numeric_limits<double>::infinity();
for (int i = 0; i < 3; i++) {
if (fabs(v[i]) < minval) {
mincoord = i;
minval = fabs(v[i]);
}
}
Eigen::Vector3d axis(0, 0, 0);
axis[mincoord] = 1.0;
result = axis.cross(v);
result /= result.norm();
return result;
}
Eigen::Vector3d parallelDerivative(const Eigen::Vector3d &v,
const Eigen::Vector3d &e1,
const Eigen::Vector3d &e2,
const Eigen::Matrix3d &rot)
{
Eigen::Vector3d t1 = e1 / e1.norm();
Eigen::Vector3d t2 = e2 / e2.norm();
Eigen::Vector3d n = t1.cross(t2);
Eigen::Vector3d dv = rot * v;
if (n.norm() < 1e-8)
return dv;
n /= n.norm();
Eigen::Vector3d p1 = n.cross(t1);
Eigen::Vector3d p2 = n.cross(t2);
double y = v.dot(n);
double z = v.dot(p1);
Eigen::Vector3d dt1 = rot * t1;
Eigen::Vector3d dn = dt1.cross(t2);
Eigen::Vector3d dp1 = dn.cross(t1) + n.cross(dt1);
Eigen::Vector3d dp2 = dn.cross(t2);
double dy = dv.dot(n) + v.dot(dn);
double dz = dv.dot(p1) + v.dot(dp1);
Eigen::Vector3d d = dy * n + y * dn + (dv.dot(t1) + v.dot(dt1)) * t2 + dz * p2 + z * dp2;
return d;
}
Eigen::Matrix3d eulerRotation(double e0, double e1, double e2, int deriv, int deriv2)
{
Eigen::Matrix3d r;
if (e0 == 0 && e1 == 0 && e2 == 0) {
switch (deriv) {
case 0:
switch (deriv2) {
case 0:
r << -1, 0, 0, 0, 0, 0, 0, 0, -1;
break;
case 1:
r << 0, 0, 0, 1, 0, 0, 0, 0, 0;
break;
case 2:
r << 0, 0, 0, 0, 0, 0, 0, 1, 0;
break;
default:
r << 0, 0, -1, 0, 0, 0, 1, 0, 0;
break;
}
break;
case 1:
switch (deriv2) {
case 0:
r << 0, 0, 0, 1, 0, 0, 0, 0, 0;
break;
case 1:
r << 0, 0, 0, 0, -1, 0, 0, 0, -1;
break;
case 2:
r << 0, 0, 0, 0, 0, 0, 1, 0, 0;
break;
default:
r << 0, 0, 0, 0, 0, 1, 0, -1, 0;
break;
}
break;
case 2:
switch (deriv2) {
case 0:
r << 0, 0, 0, 0, 0, 0, 0, 1, 0;
break;
case 1:
r << 0, 0, 0, 0, 0, 0, 1, 0, 0;
break;
case 2:
r << -1, 0, 0, 0, -1, 0, 0, 0, 0;
break;
default:
r << 0, 1, 0, -1, 0, 0, 0, 0, 0;
break;
}
break;
default:
r << 1, 0, 0, 0, 1, 0, 0, 0, 1;
break;
}
} else {
switch (deriv) {
case 0:
switch (deriv2) {
case 0:
r << -cos(e0) * cos(e2), -cos(e0) * sin(e2), sin(e0), sin(e1) * -sin(e0) * cos(e2),
sin(e1) * -sin(e0) * sin(e2), -cos(e0) * sin(e1), cos(e1) * -sin(e0) * cos(e2),
cos(e1) * -sin(e0) * sin(e2), -cos(e0) * cos(e1);
break;
case 1:
r << 0, 0, 0, cos(e1) * cos(e0) * cos(e2), cos(e1) * cos(e0) * sin(e2),
-sin(e0) * cos(e1), -sin(e1) * cos(e0) * cos(e2), -sin(e1) * cos(e0) * sin(e2),
-sin(e0) * -sin(e1);
break;
case 2:
r << -sin(e0) * -sin(e2), -sin(e0) * cos(e2), 0, sin(e1) * cos(e0) * -sin(e2),
sin(e1) * cos(e0) * cos(e2), 0, cos(e1) * cos(e0) * -sin(e2),
cos(e1) * cos(e0) * cos(e2), 0;
break;
default:
r << -sin(e0) * cos(e2), -sin(e0) * sin(e2), -cos(e0), sin(e1) * cos(e0) * cos(e2),
sin(e1) * cos(e0) * sin(e2), -sin(e0) * sin(e1), cos(e1) * cos(e0) * cos(e2),
cos(e1) * cos(e0) * sin(e2), -sin(e0) * cos(e1);
break;
}
break;
case 1:
switch (deriv2) {
case 0:
r << 0, 0, 0, cos(e1) * cos(e0) * cos(e2), cos(e1) * cos(e0) * sin(e2),
-sin(e0) * cos(e1), -sin(e1) * cos(e0) * cos(e2), -sin(e1) * cos(e0) * sin(e2),
-sin(e0) * -sin(e1);
break;
case 1:
r << 0, 0, 0, -sin(e1) * sin(e0) * cos(e2) - -cos(e1) * sin(e2),
-sin(e1) * sin(e0) * sin(e2) + -cos(e1) * cos(e2), cos(e0) * -sin(e1),
-cos(e1) * sin(e0) * cos(e2) + -sin(e1) * sin(e2),
-cos(e1) * sin(e0) * sin(e2) - -sin(e1) * cos(e2), cos(e0) * -cos(e1);
break;
case 2:
r << 0, 0, 0, cos(e1) * sin(e0) * -sin(e2) - -sin(e1) * cos(e2),
cos(e1) * sin(e0) * cos(e2) + -sin(e1) * -sin(e2), 0,
-sin(e1) * sin(e0) * -sin(e2) + cos(e1) * cos(e2),
-sin(e1) * sin(e0) * cos(e2) - cos(e1) * -sin(e2), 0;
break;
default:
r << 0, 0, 0, cos(e1) * sin(e0) * cos(e2) - -sin(e1) * sin(e2),
cos(e1) * sin(e0) * sin(e2) + -sin(e1) * cos(e2), cos(e0) * cos(e1),
-sin(e1) * sin(e0) * cos(e2) + cos(e1) * sin(e2),
-sin(e1) * sin(e0) * sin(e2) - cos(e1) * cos(e2), cos(e0) * -sin(e1);
break;
}
break;
case 2:
switch (deriv2) {
case 0:
r << -sin(e0) * -sin(e2), -sin(e0) * cos(e2), 0, sin(e1) * cos(e0) * -sin(e2),
sin(e1) * cos(e0) * cos(e2), 0, cos(e1) * cos(e0) * -sin(e2),
cos(e1) * cos(e0) * cos(e2), 0;
break;
case 1:
r << 0, 0, 0, cos(e1) * sin(e0) * -sin(e2) - -sin(e1) * cos(e2),
cos(e1) * sin(e0) * cos(e2) + -sin(e1) * -sin(e2), 0,
-sin(e1) * sin(e0) * -sin(e2) + cos(e1) * cos(e2),
-sin(e1) * sin(e0) * cos(e2) - cos(e1) * -sin(e2), 0;
break;
case 2:
r << cos(e0) * -cos(e2), cos(e0) * -sin(e2), 0,
sin(e1) * sin(e0) * -cos(e2) - cos(e1) * -sin(e2),
sin(e1) * sin(e0) * -sin(e2) + cos(e1) * -cos(e2), 0,
cos(e1) * sin(e0) * -cos(e2) + sin(e1) * -sin(e2),
cos(e1) * sin(e0) * -sin(e2) - sin(e1) * -cos(e2), 0;
break;
default:
r << cos(e0) * -sin(e2), cos(e0) * cos(e2), 0,
sin(e1) * sin(e0) * -sin(e2) - cos(e1) * cos(e2),
sin(e1) * sin(e0) * cos(e2) + cos(e1) * -sin(e2), 0,
cos(e1) * sin(e0) * -sin(e2) + sin(e1) * cos(e2),
cos(e1) * sin(e0) * cos(e2) - sin(e1) * -sin(e2), 0;
break;
}
break;
default:
r << cos(e0) * cos(e2), cos(e0) * sin(e2), -sin(e0),
sin(e1) * sin(e0) * cos(e2) - cos(e1) * sin(e2),
sin(e1) * sin(e0) * sin(e2) + cos(e1) * cos(e2), cos(e0) * sin(e1),
cos(e1) * sin(e0) * cos(e2) + sin(e1) * sin(e2),
cos(e1) * sin(e0) * sin(e2) - sin(e1) * cos(e2), cos(e0) * cos(e1);
break;
}
}
return r;
}
Eigen::Vector3d perpToVectorAxis(const Eigen::Vector3d &v)
{
Eigen::Vector3d result;
int mincoord = 0;
double minval = std::numeric_limits<double>::infinity();
for (int i = 0; i < 3; i++) {
if (fabs(v[i]) < minval) {
mincoord = i;
minval = fabs(v[i]);
}
}
Eigen::Vector3d axis(0, 0, 0);
axis[mincoord] = 1.0;
return axis;
}
double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2, const Eigen::Vector3d &axis)
{
double result = 2.0 * atan2((v1.cross(v2)).dot(axis), v1.norm() * v2.norm() + v1.dot(v2));
return result;
}
Eigen::Matrix3d skewSym(const Eigen::Vector3d &v)
{
Eigen::Matrix3d M;
M << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
return M;
}
Eigen::Vector3d parallelTransport(const Eigen::Vector3d &v,
const Eigen::Vector3d &e1,
const Eigen::Vector3d &e2)
{
Eigen::Vector3d t1 = e1 / e1.norm();
Eigen::Vector3d t2 = e2 / e2.norm();
Eigen::Vector3d n = t1.cross(t2);
if (n.norm() < 1e-8)
return v;
n /= n.norm();
Eigen::Vector3d p1 = n.cross(t1);
Eigen::Vector3d p2 = n.cross(t2);
return v.dot(n) * n + v.dot(t1) * t2 + v.dot(p1) * p2;
}
SimulationResults DER_leimer::constructSimulationResults(const std::shared_ptr<SimulationJob> &pJob,
RodConfig *config) const
{
SimulationResults simulationResults;
simulationResults.displacements.resize(config->numVertices());
simulationResults.converged = true;
// pJob->pMesh->registerForDrawing();
// polyscope::show();
for (int vi = 0; vi < config->numVertices(); vi++) {
// std::cout << vi << " pos:" << config->vertices(vi, 0) << " " << config->vertices(vi, 1)
// << " " << config->vertices(vi, 2) << std::endl;
for (int dofi = 0; dofi < 3; dofi++) {
const double initialPos_dofi = pJob->pMesh->vert[vi].cP()[dofi];
const double deformedPos_dofi = config->vertices(vi, dofi);
const double displacement_dofi = deformedPos_dofi - initialPos_dofi;
simulationResults.displacements[vi][dofi] = displacement_dofi;
}
for (int dofi = 3; dofi < 6; dofi++) {
simulationResults.displacements[vi][dofi] = 0;
}
}
simulationResults.rotationalDisplacementQuaternion.resize(config->numVertices(),
Eigen::Quaternion<double>::Identity());
// std::cerr << "There is no information regarding the rotational displacements of the vertices "
// "in the simulation results. Using identity rotation for all vertices.."
// << std::endl;
// for (int intersectionIndex = 0; intersectionIndex < config.numIntersections(); intersectionIndex++) { //iterate over all intersections
// config->numIntersections()
// }
simulationResults.pJob = pJob;
return simulationResults;
}
SimulationResults DER_leimer::executeSimulation(const std::shared_ptr<SimulationJob> &pJob)
{
this->pJob = pJob;
// constexpr int numRods = 3;
// constexpr int numVertsPerRod = 11;
// std::vector<int> vInd = {0, 1, 2, 0, 3, 4, 0, 5, 6, 0, 7, 8};
// //t-junction 10 elems
// std::vector<int> vInd = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 0, 11, 12, 13, 14, 15,
// 16, 17, 18, 19, 20, 0, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30};
// std::vector<Eigen::Vector3d> verts{{0, 0, 0}, {0, 0.1, 0}, {0, 0.2, 0}, {0, 0.3, 0},
// {0, 0.4, 0}, {0, 0.5, 0}, {0, 0.6, 0}, {0, 0.7, 0},
// {0, 0.8, 0}, {0, 0.9, 0}, {0, 1, 0}, {0.1, 0, 0},
// {0.2, 0, 0}, {0.3, 0, 0}, {0.4, 0, 0}, {0.5, 0, 0},
// {0.6, 0, 0}, {0.7, 0, 0}, {0.8, 0, 0}, {0.9, 0, 0},
// {1, 0, 0}, {0, -0.1, 0}, {0, -0.2, 0}, {0, -0.3, 0},
// {0, -0.4, 0}, {0, -0.5, 0}, {0, -0.6, 0}, {0, -0.7, 0},
// {0, -0.8, 0}, {0, -0.9, 0}, {0, -1, 0}};
// Anchor a0;
// a0.rod = 0;
// a0.seg = 9;
// a0.bary = 1;
// a0.position = Eigen::Vector3d(0, 1, 0);
// a0.normal = Eigen::Vector3d(0, 0, 1);
// a0.normal = Eigen::Vector3d(1, 0, 0);
// a0.normal = (a0.position.cross(Eigen::Vector3d(1, 1, 1).normalized())).normalized();
// Anchor a1;
// a1.rod = 2;
// a1.seg = 9;
// a1.bary = 1;
// a1.position = Eigen::Vector3d(0, -1, 0);
// a1.normal = Eigen::Vector3d(0, 0, 1);
// a1.normal = Eigen::Vector3d(1, 0, 0);
// Anchor a3;
// a3.rod = 0;
// a3.seg = 8;
// a3.bary = 1;
// a3.position = Eigen::Vector3d(0, 0.9, 0);
// a3.normal = Eigen::Vector3d(1, 0, 0);
// Anchor a4;
// a4.rod = 2;
// a4.seg = 8;
// a4.bary = 1;
// a4.position = Eigen::Vector3d(0, -0.9, 0);
// a4.normal = Eigen::Vector3d(1, 0, 0);
// std::vector<Anchor> anchors{a0, a1 /*, a3, a4*/ /*, a2*/};
// //cantilever 2-elems
// constexpr int numRods = 1;
// constexpr int numVertsPerRod = 3;
// std::vector<int> vInd{0, 1, 2};
// std::vector<Eigen::Vector3d> verts{{0, 0, 0}, {0.5, 0, 0}, {1, 0, 0}};
// Anchor a0;
// a0.rod = 0;
// a0.seg = 0;
// a0.bary = 0;
// a0.position = Eigen::Vector3d(0, 0, 0);
// a0.normal = Eigen::Vector3d(0, 0, 1);
// std::vector<Anchor> anchors{a0};
// //cantilever 4-elems
// constexpr int numRods = 1;
// constexpr int numVertsPerRod = 5;
// std::vector<int> vInd{0, 1, 2, 3, 4};
// std::vector<Eigen::Vector3d> verts{{0, 0, 0}, {0.25, 0, 0}, {0.5, 0, 0}, {0.75, 0, 0}, {1, 0, 0}};
// Anchor a0;
// a0.rod = 0;
// a0.seg = 0;
// a0.bary = 0;
// a0.position = Eigen::Vector3d(0, 0, 0);
// a0.normal = Eigen::Vector3d(0, 0, 1);
// std::vector<Anchor> anchors{a0};
const std::shared_ptr<VCGEdgeMesh> pMesh = pJob->pMesh;
const int numRods = pMesh->EN();
constexpr int numVertsPerRod = 2;
std::vector<int> vInd;
vInd.reserve(pMesh->EN());
for (int ei = 0; ei < pMesh->EN(); ei++) {
const int vi0 = pMesh->getIndex(pMesh->edge[ei].cV(0));
vInd.push_back(vi0);
const int vi1 = pMesh->getIndex(pMesh->edge[ei].cV(1));
vInd.push_back(vi1);
}
// vInd.erase(std::unique(vInd.begin(), vInd.end()), vInd.end());
std::vector<Eigen::Vector3d> verts(pJob->pMesh->VN());
for (int vi = 0; vi < pJob->pMesh->VN(); vi++) {
verts[vi] = pJob->pMesh->vert[vi].cP().ToEigenVector<Eigen::Vector3d>();
}
std::vector<Anchor> anchors;
anchors.reserve(pJob->constrainedVertices.size());
for (const auto bc : pJob->constrainedVertices) {
std::unordered_set<int> rigidBC({0, 1, 2, 3, 4, 5});
std::unordered_set<int> fixedBC({0, 1, 2});
assert(bc.second == rigidBC || bc.second == fixedBC);
const EdgePointer ep = pMesh->vert[bc.first].VEp();
Anchor a;
a.rod = pMesh->getIndex(ep);
a.seg = 0;
const int constrainedVi = bc.first;
const int edgeVi = [&]() {
if (ep->cV(0) == &pMesh->vert[constrainedVi]) {
return 0;
} else {
assert(ep->cV(1) == &pMesh->vert[constrainedVi]);
return 1;
}
}();
a.bary = edgeVi;
a.position = ep->cP(edgeVi).ToEigenVector<Eigen::Vector3d>();
a.normal = ep->cV(edgeVi)->cN().ToEigenVector<Eigen::Vector3d>();
a.k_pos = 1e6;
if (bc.second == fixedBC) {
a.k_dir = 0;
} else {
assert(bc.second == rigidBC);
a.k_dir = 1e6;
}
anchors.push_back(a);
}
// constexpr int numRods = 1;
// constexpr int numVertsPerRod = 11;
// std::vector<int> vInd{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
// std::vector<Eigen::Vector3d> verts{{0, 0, 0},
// {0.1, 0, 0},
// {0.2, 0, 0},
// {0.3, 0, 0},
// {0.4, 0, 0},
// {0.5, 0, 0},
// {0.6, 0, 0},
// {0.7, 0, 0},
// {0.8, 0, 0},
// {0.9, 0, 0},
// {1, 0, 0}};
// Anchor a0;
// a0.rod = 0;
// a0.seg = 0;
// a0.bary = 0;
// a0.position = Eigen::Vector3d(0, 0, 0);
// a0.normal = Eigen::Vector3d(0, 0, 1);
// std::vector<Anchor> anchors{a0};
RodParams rodParams;
const double E = pJob->pMesh->getBeamMaterial()[0].youngsModulus;
const double w = pJob->pMesh->getBeamDimensions()[0].getDim1();
const double h = pJob->pMesh->getBeamDimensions()[0].getDim2();
rodParams.thickness = h;
rodParams.kbending = E;
rodParams.kstretching = E;
const double poissonsRatio = 0.3;
rodParams.ktwist = E / (2 * (1 + poissonsRatio));
rodParams.kanchorpos = 1e4;
// rodParams.kanchordir = 0;
rodParams.kanchordir = 1e4;
rodParams.restlength = -1;
rodParams.rho = 1e0;
std::vector<RodParams> rodParamsPerRod(numRods, rodParams);
const int numSegments = (numVertsPerRod - 1) * numRods;
std::vector<Eigen::Vector3d> binorms(numSegments, Eigen::Vector3d(0, 0, 1));
std::vector<double> segmentWidths(numSegments, w);
// assert(verts.size() == numGlobalVertices);
assert(vInd.size() == numRods * numVertsPerRod);
const std::vector<int> numVerticesPerRod(numRods, numVertsPerRod);
RodConfig *config = readRodGrid(pJob->pMesh,
numVerticesPerRod,
vInd,
verts,
binorms,
rodParamsPerRod,
anchors);
// assert(config->numIntersections() == 0);
// pRod = std::make_unique<ElasticRod>(verts);
// const auto cs = CrossSection::construct("rectangle", E, poissonsRatio, {w, h});
// RodMaterial mat(*cs);
// pRod->setMaterial(mat);
bool done = false;
while (!done) {
done = simulateOneStepGrid(config);
}
SimulationResults simulationResults = constructSimulationResults(pJob, config);
return simulationResults;
}
bool DER_leimer::simulateOneStepGrid(RodConfig *config)
{
//Define external forces
Eigen::VectorXd externalForces;
// externalForces.setZero(
// config->getNumDoF()); //dof:3*config.verts+numSegments+numConstraints+3*numIntersections
// constexpr int externalForceGlobalVi = 2;
// constexpr int vertexPosDof = 2;
// constexpr double forceMagnitude = -1e-5;
// externalForces(3 * externalForceGlobalVi + vertexPosDof) = forceMagnitude;
// externalForces(3 * 4 + vertexPosDof) = -forceMagnitude;
// externalForces(3 * 8 + vertexPosDof) = -forceMagnitude;
// externalForces(3 * 9 + 5) = forceMagnitude;
// externalForces(3 * 6 + vertexPosDof) = forceMagnitude;
double externalForcesPE = 0;
//testFiniteDifferences();
Eigen::VectorXd r;
Eigen::SparseMatrix<double> Jr;
double linE;
Eigen::VectorXd gravityForces;
Eigen::MatrixXd anchorPoints;
Eigen::MatrixXd anchorNormals;
// bool useanchors = false;
// if (stickToMesh && targetV.rows() > 0) {
// findAnchorPoints(anchorPoints, anchorNormals);
// useanchors = true;
// }
// if (allowSliding) {
// for (int i = 0; i < config->constraints.size(); i++) {
// Constraint &c = config->constraints[i];
// c.prevSeg1 = c.seg1;
// c.prevSeg2 = c.seg2;
// c.prevBary1 = c.bary1;
// c.prevBary2 = c.bary2;
// }
// }
SimParams params;
params.anchorPoints = NULL;
params.anchorNormals = NULL;
params.gravityEnabled = true;
params.gravityDir = Eigen::Vector3d(0, 0, 1);
params.floorHeight = 0;
params.floorWeight = 1e-4;
rAndJGrid(*config, r, &Jr, linE, gravityForces, params, externalForcesPE, externalForces);
energy = 0.5 * r.transpose() * r + linE /*- externalForcesPE*/;
std::cout << "energy: " << energy << std::endl;
Eigen::SparseMatrix<double> mat = Jr.transpose() * Jr;
const bool shouldRegularize = [&]() {
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(mat);
if (solver.info() != Eigen::Success) {
return true;
}
return false;
}();
if (shouldRegularize) {
Eigen::SparseMatrix<double> I(mat.rows(), mat.cols());
I.setIdentity();
double Treg = 1e-6;
mat += Treg * I;
}
// std::string sep = "\n----------------------------------------\n";
// Eigen::IOFormat CommaInitFmt(2,
// 0,
// ",",
// "\n"/*,
// "",
// "",
// " << ",
// ";"*/);
// std::ofstream file("Jr.csv");
// Eigen::MatrixXd Jr_dense(Jr);
// Eigen::MatrixXd mat_dense(mat);
// if (file.is_open()) {
// file << Jr_dense.format(CommaInitFmt) << sep;
// }
const Eigen::VectorXd internalForces = Jr.transpose() * r;
Eigen::VectorXd rhs = Jr.transpose() * r + gravityForces /*- externalForces*/;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(mat);
if (solver.info() != Eigen::Success) {
std::cerr << "Unsuccessful factorization" << std::endl;
}
Eigen::VectorXd delta = solver.solve(rhs);
if (updateVec.isZero())
updateVec = delta;
else
updateVec = (1.0 - config->decay) * updateVec + config->decay * delta;
if (solver.info() != Eigen::Success) {
std::cerr << "Error while solving for the displacement vector" << std::endl;
exit(-1);
}
const double solverResidual = (mat * delta - rhs).norm();
std::cout << "Solver residual: " << solverResidual << std::endl;
forceResidual = lineSearchGrid(*config, updateVec, params, externalForces);
std::cout << "Force residual: " << forceResidual << std::endl;
//panetta's rod update
// assert(config->numIntersections() == 0);
std::vector<Eigen::Vector3d> vertexPositions(config->numVertices());
for (int vi = 0; vi < config->numVertices(); vi++) {
vertexPositions[vi] = config->vertices.row(vi);
}
std::vector<double> thetas(config->numRods());
for (int ri = 0; ri < config->numRods(); ri++) {
assert(config->rods[ri]->numSegments() == 1);
thetas[ri] = config->rods[ri]->curState.thetas[0];
}
// pRod->setDeformedConfiguration(vertexPositions, thetas);
rAndJGrid(*config, r, &Jr, linE, gravityForces, params, externalForcesPE, externalForces);
iter++;
if (iter > 100 /*|| solverResidual < 1e-10*/ || forceResidual < 1e-5) {
// for (int vi = 0; vi < config->numVertices(); vi++) {
// std::cout << vi << " pos:" << config->vertices(vi, 0) << " " << config->vertices(vi, 1)
// << " " << config->vertices(vi, 2) << std::endl;
// }
std::cout << "Converged" << std::endl;
return true;
}
return false;
}
void DER_leimer::rAndJGrid(const RodConfig &config,
Eigen::VectorXd &r,
Eigen::SparseMatrix<double> *Jr,
double &gravityPE,
Eigen::VectorXd &gravityForces,
const SimParams &params,
double &externalForcesPE,
const Eigen::VectorXd &externalForces,
std::vector<Eigen::Triplet<int>> *H_index,
std::vector<double> *H_value,
Eigen::SparseMatrix<double> *Ju,
std::vector<Eigen::Triplet<int>> *Hu_index,
std::vector<double> *Hu_value)
{
// const bool computeHessian = [&]() {
// if (H_index && H_value && Ju && Hu_index && Hu_value)
// return true;
// return false;
// }();
const bool hasExternalForces = externalForces.rows() != 0;
int nterms = 0;
int ndofs = 0;
int nsegsTotal = 0;
// int nconstraints = 0;
std::vector<int> nDofsPerRod(config.numRods());
std::vector<int> numAccumDofsPerRod(config.numRods());
for (int i = 0; i < config.numRods(); i++) {
nDofsPerRod[i] = config.rods[i]->numSegments();
ndofs += nDofsPerRod[i];
nsegsTotal += config.rods[i]->numSegments();
numAccumDofsPerRod[i] = ndofs;
// std::cout << "nDofsPerRodAccum[" << i << "] = " << nDofsPerRodAccum[i] << std::endl;
int njoints = config.rods[i]->isClosed() ? config.rods[i]->numSegments()
: (config.rods[i]->numSegments() - 1);
nterms += config.rods[i]->numSegments(); // stretching
nterms += 2 * njoints; // bending
nterms += njoints; // twisting
#ifndef EXPLICIT_BC
if (params.anchorPoints) //??
nterms += 3 * config.rods[i]->numVertices();
#endif
// if (hasExternalForces) {
// nterms += config.rods[i]->numVertices();
// }
if (params.gravityEnabled) {
nterms += config.rods[i]->numVertices(); // floor force
}
}
ndofs += config.numVertices() * 3; // DOF for vertices
if (params.anchorPoints)
nterms += 3 * config.numVertices();
//if (params.gravityEnabled)
// nterms += config.numVertices(); // floor force
#ifndef EXPLICIT_BC
nterms += 4 * config.numAnchors();
#endif
// Barycentric Coordinates are handled as DOF's since they can change when rods are sliding
int baryoffset = ndofs;
// ndofs += 2 * config.numConstraints(); //??
// nterms += 6 * config.constraints.size(); // constraint positions
// nterms += config.constraints.size(); // constraint directions
// nterms += 4 * config.constraints.size(); // barycentric coord inequality constraints
// energy terms for rod intersections
for (int i = 0; i < config.numIntersections(); i++) {
const Intersection &intersection = config.intersections[i];
for (int rodSegmentIndex = 0; rodSegmentIndex < intersection.rodSegmentIndices.rows();
rodSegmentIndex++) {
const int rodIndex = intersection.rodSegmentIndices(rodSegmentIndex, 0);
const int segIndex = intersection.rodSegmentIndices(rodSegmentIndex, 1);
const Rod &rod = *config.rods[rodIndex];
if (segIndex > 0 || rod.isClosed())
nterms += 3; //??
if (segIndex < rod.numVertices() - 1 || rod.isClosed())
nterms += 3; //??
}
}
ndofs += 3 * config.numIntersections(); // euler angles as DOFs for intersections
r.resize(nterms);
// r.setConstant(std::numeric_limits<double>::infinity());
r.setZero();
gravityPE = 0;
gravityForces.resize(ndofs);
gravityForces.setZero();
if (hasExternalForces) {
assert(externalForces.rows() == gravityForces.rows());
externalForcesPE = 0;
}
if (Jr) {
Jr->resize(nterms, ndofs);
}
std::vector<int> indexOfIntersection(config.numVertices(), -1);
for (int i = 0; i < config.numIntersections(); i++) {
indexOfIntersection[config.intersections[i].globalVi] = config.intersections[i]
.intersectionIndex;
}
std::vector<Eigen::Triplet<double>> J;
std::vector<Eigen::Triplet<double>> Jui; // jacobian for u-variables. What are "u-variables"?
int uianchoroffset = 0;
int uistiffoffset = uianchoroffset + 6 * config.numAnchors();
int udofs = uistiffoffset + 5 * nsegsTotal;
int roffset = 0;
int dofoffset = 0;
int thetaoffset = 3 * config.numVertices();
int euleroffset = config.numVertices() * 3 + numAccumDofsPerRod[config.numRods() - 1];
for (int rodIndex = 0; rodIndex < config.numRods(); rodIndex++) {
const RodState &rodState_current = config.rods[rodIndex]->curState;
RodState &rodState_initial = config.rods[rodIndex]->startState;
const Rod &rod = *config.rods[rodIndex];
const int numVertices_rod = (int) rodState_current.centerlinePositions.rows();
const int numSegments_rod = rod.numSegments();
bool computeRestCurvatures = false;
if (rodState_initial.curvatures.size() == 0) {
computeRestCurvatures = true;
rodState_initial.curvatures.resize(numSegments_rod, 2);
}
// //Computation of PE of External forces. Need to change that when joints are added
// if (hasExternalForces) {
// //TODO:This are only translational forces since I am iterating over the rod vertices. I should add also add moments. Check the other implementations of der to see how they did that
// for (int vi_rod = 0; vi_rod < numVertices_rod; vi_rod++) {
// const Eigen::Vector3d displacementVector
// = rodState_current.centerlinePositions.row(vi_rod).transpose()
// - rodState_initial.centerlinePositions.row(vi_rod).transpose();
// const int globalVi = rodState_current.centerlineIndices[vi_rod];
// const int vertexOffset = 3 * globalVi;
// const Eigen::Vector3d externalForce_vertex(externalForces(vertexOffset),
// externalForces(vertexOffset + 1),
// externalForces(vertexOffset + 2));
// const double pe_externalForce = externalForce_vertex.dot(displacementVector);
// externalForcesPE += pe_externalForce;
// // r[roffset + vi_rod] = std::sqrt(pe_externalForce);
// r[roffset + vi_rod] = 0;
// if (Jr) {
// for (int j = 0; j < 3; j++)
// J.push_back(Eigen::Triplet<double>(roffset + vi_rod,
// vertexOffset + j,
// /*-sqrt(externalForce_vertex[j])*/ 0));
// }
// }
// roffset += numVertices_rod;
// }
// // gravity terms
// // Note: if a vertex is shared by multiple rods, the constraint is applied once per rod currently
// if (params.gravityEnabled) {
// for (int i = 0; i < numVertices_rod; i++) {
// int m = rodState_current.centerlineIndices[i];
// Eigen::Vector3d pos = rodState_current.centerlinePositions.row(i).transpose();
// double mass = rod.masses[i];
// double potential = pos.dot(params.gravityDir) * gravityG * mass
// / params.gravityDir.norm();
// gravityPE += potential;
// for (int j = 0; j < 3; j++) {
// gravityForces[3 * m + j] += params.gravityDir[j] * gravityG * mass
// / params.gravityDir.norm();
// }
// double pendist = params.floorHeight
// - pos.dot(params.gravityDir) / params.gravityDir.norm();
// if (pendist > 0) {
// r[roffset + i] = sqrt(params.floorWeight) * pendist;
// if (Jr) {
// for (int j = 0; j < 3; j++)
// J.push_back(Eigen::Triplet<double>(roffset + i,
// 3 * m + j,
// -sqrt(params.floorWeight)
// * params.gravityDir[j]
// / params.gravityDir.norm()));
// }
// } else {
// r[roffset + i] = 0;
// }
// }
// roffset += numVertices_rod;
// }
// gravity terms
// Note: if a vertex is shared by multiple rods, the constraint is applied once per rod currently
if (params.gravityEnabled) {
// const Eigen::Vector3d externalForce(0, 0, -1e-3);
// const Eigen::Vector3d externalForce(0, 0, 0);
// const Eigen::Vector3d externalForce(-1e4, 0, 0);
// const Eigen::Vector3d externalForce(0, 0, -1e-4);
Eigen::Vector3d externalForce(0, 0, 0);
for (int i = 0; i < numVertices_rod; i++) {
int m = rodState_current.centerlineIndices[i];
const Eigen::Vector3d currentPos = rodState_current.centerlinePositions.row(i)
.transpose();
const Eigen::Vector3d initialPos
= pJob->pMesh->vert[m].cP().ToEigenVector<Eigen::Vector3d>();
const Eigen::Vector3d displacement = currentPos - initialPos;
r[roffset + i] = 0;
if (pJob->nodalExternalForces.contains(m)) {
externalForce = -pJob->nodalExternalForces[m].getTranslation();
}
// switch (m) {
// // case 16:
// // externalForce = Eigen::Vector3d(0, -1e-2, 0);
// // break;
// // case 20:
// // case 9:
// case 4:
// // case 5:
// // case 0:
// // externalForce = Eigen::Vector3d(-1e0, 0, 0);
// externalForce = Eigen::Vector3d(0, 0, -1e-2);
// break;
// // case 16:
// // externalForce = Eigen::Vector3d(0, 0, -1e-3);
// // 3reak;
// default:
// continue;
// break;
// }
// std::cout << "x:" << pos[0] << " y:" << pos[1] << " z:" << pos[2] << std::endl;
double potential = displacement.dot(externalForce);
gravityPE += potential;
for (int j = 0; j < 3; j++) {
gravityForces[3 * m + j] += externalForce(j);
}
}
roffset += numVertices_rod;
}
double energy_stretch = 0;
// stretching terms
for (int si_rod = 0; si_rod < numSegments_rod; si_rod++) {
const int m1 = rodState_current.centerlineIndices[si_rod];
const int m2 = rodState_current.centerlineIndices[si_rod + 1 % numVertices_rod];
const Eigen::Vector3d v1 = rodState_current.centerlinePositions.row(si_rod).transpose();
const Eigen::Vector3d v2 = rodState_current.centerlinePositions
.row((si_rod + 1) % numVertices_rod)
.transpose();
const double len = (v1 - v2).norm();
const double restlen = rod.vSegParams(si_rod, 3);
const double stiffness_stretch = rod.vSegParams(si_rod, 0) * rod.params.thickness
* rod.widths[si_rod];
const double factor = /*0.5 **/ rod.params.rho * stiffness_stretch / restlen;
const double segr = sqrt(factor)
* (len - restlen); // (square root of?) stretching energy Es
r[roffset + si_rod] = segr;
// const double segmentArea = rod.params.thickness * rod.widths[si_rod];
// const double strain_segment = len / restlen - 1;
// const double debug_segmentEnergyStretchPanetta = 0.5 * rod.params.rho
// * stiffness_stretch * segmentArea
// * strain_segment * strain_segment
// * restlen;
// energy_stretch += debug_segmentEnergyStretchPanetta;
// const double segr_sqrd = segr * segr;
// const double debug_energyStretchLeimer = 0.5 * segr_sqrd;
// assert(std::abs(debug_energyStretchLeimer - debug_segmentEnergyStretchPanetta) < 1e-5);
if (Jr) {
const Eigen::Vector3d dlen = (v1 - v2).normalized();
// const double coeff = rod.params.rho * stiffness_stretch * segmentArea
// * strain_segment;
// const Eigen::Vector3d debug_forcePanetta_j = -coeff * dlen;
const Eigen::Vector3d jacobianEntry
= sqrt(factor) * dlen; //needs to be (63.25,0,0) instead of (44.72,0,0)
// const Eigen::Vector3d debug_forceLeimer = jacobianEntry * segr;
// assert(debug_forceLeimer == debug_forcePanetta_j);
for (int j = 0; j < 3; j++) {
// Gradients for dEs/dx_i and dEs/dx_(i+1)
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(
Eigen::Triplet<double>(roffset + si_rod, 3 * m1 + j, jacobianEntry[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m2)) {
#endif
J.push_back(
Eigen::Triplet<double>(roffset + si_rod, 3 * m2 + j, -jacobianEntry[j]));
#ifdef EXPLICIT_BC
}
#endif
}
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
Eigen::Matrix3d Hlen = (1 / len) * Eigen::Matrix<double, 3, 3>::Identity()
- (1 / (len * len * len)) * (v1 - v2)
* (v1 - v2).transpose();
double df_dks = (rod.params.thickness * rod.widths[si_rod])
/ (4 * restlen * sqrt(factor));
double df_dw = (rod.params.thickness * rod.vSegParams(si_rod, 0))
/ (4 * restlen * sqrt(factor));
double df_dt = (rod.vSegParams(si_rod, 0) * rod.widths[si_rod])
/ (4 * restlen * sqrt(factor));
for (int h1 = 0; h1 < 3; h1++) {
for (int h2 = 0; h2 < 3; h2++) {
H_index->push_back(
Eigen::Triplet<int>(roffset + si_rod, 3 * m1 + h1, 3 * m1 + h2));
H_value->push_back(sqrt(factor) * Hlen(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + si_rod, 3 * m1 + h1, 3 * m2 + h2));
H_value->push_back(-sqrt(factor) * Hlen(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + si_rod, 3 * m2 + h1, 3 * m1 + h2));
H_value->push_back(-sqrt(factor) * Hlen(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + si_rod, 3 * m2 + h1, 3 * m2 + h2));
H_value->push_back(sqrt(factor) * Hlen(h1, h2));
}
// derivatives stiffness parameters
Hu_index->push_back(Eigen::Triplet<int>(roffset + si_rod,
3 * m1 + h1,
uistiffoffset + 0 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dks * dlen[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + si_rod,
3 * m2 + h1,
uistiffoffset + 0 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(-df_dks * dlen[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + si_rod,
3 * m1 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dw * dlen[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + si_rod,
3 * m2 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(-df_dw * dlen[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + si_rod,
3 * m1 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dt * dlen[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + si_rod,
3 * m2 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(-df_dt * dlen[h1]);
}
Jui.push_back(
Eigen::Triplet<double>(roffset + si_rod,
uistiffoffset + 0 * nsegsTotal + dofoffset + si_rod,
df_dks * (len - restlen)));
Jui.push_back(
Eigen::Triplet<double>(roffset + si_rod,
uistiffoffset + 3 * nsegsTotal + dofoffset + si_rod,
df_dw * (len - restlen)));
Jui.push_back(
Eigen::Triplet<double>(roffset + si_rod,
uistiffoffset + 4 * nsegsTotal + dofoffset + si_rod,
df_dt * (len - restlen)));
}
#endif
}
}
roffset += rod.numSegments();
// bending terms
int startseg = rod.isClosed() ? 0 : 1;
int idx = 0;
for (int si_rod = startseg; si_rod < numSegments_rod; si_rod++) {
int m0 = rodState_current
.centerlineIndices[(numVertices_rod + si_rod - 1) % numVertices_rod];
int m1 = rodState_current.centerlineIndices[si_rod];
int m2 = rodState_current.centerlineIndices[(si_rod + 1) % numVertices_rod];
if (indexOfIntersection[m1] < 0) {
// compute curvature of rest state
if (computeRestCurvatures) {
Eigen::Vector3d sv0 = rodState_initial.centerlinePositions
.row(/*(numVertices_rod*/ +si_rod
- 1 /*) % numVertices_rod*/)
.transpose();
Eigen::Vector3d sv1 = rodState_initial.centerlinePositions.row(si_rod)
.transpose();
Eigen::Vector3d sv2 = rodState_initial.centerlinePositions
.row((si_rod + 1) % numVertices_rod)
.transpose();
Eigen::Vector3d st01 = (sv1 - sv0) / (sv1 - sv0).norm();
Eigen::Vector3d st12 = (sv2 - sv1) / (sv2 - sv1).norm();
Eigen::Vector3d skb = 2.0 * st01.cross(st12) / (1.0 + st01.dot(st12));
Eigen::Vector3d sdb11 = rodState_initial.directors.row(
(numSegments_rod + si_rod - 1) % numSegments_rod);
Eigen::Vector3d sdb21 = st01.cross(sdb11);
Eigen::Vector3d sdb12 = rodState_initial.directors.row(si_rod);
Eigen::Vector3d sdb22 = st12.cross(sdb12);
double stheta1 = rodState_initial
.thetas[(numSegments_rod + si_rod - 1) % numSegments_rod];
double stheta2 = rodState_initial.thetas[si_rod];
Eigen::Vector3d sd11 = sdb11 * cos(stheta1) + sdb21 * sin(stheta1);
Eigen::Vector3d sd21 = -sdb11 * sin(stheta1) + sdb21 * cos(stheta1);
Eigen::Vector3d sd12 = sdb12 * cos(stheta2) + sdb22 * sin(stheta2);
Eigen::Vector3d sd22 = -sdb12 * sin(stheta2) + sdb22 * cos(stheta2);
rodState_initial.curvatures(si_rod, 0) = 0.5 * ((sd21 + sd22).dot(skb));
rodState_initial.curvatures(si_rod, 1) = 0.5 * ((sd11 + sd12).dot(skb));
}
Eigen::Vector3d v0 = rodState_current.centerlinePositions
.row((numVertices_rod + si_rod - 1) % numVertices_rod)
.transpose();
Eigen::Vector3d v1 = rodState_current.centerlinePositions.row(si_rod).transpose();
Eigen::Vector3d v2 = rodState_current.centerlinePositions
.row((si_rod + 1) % numVertices_rod)
.transpose();
Eigen::Vector3d t01 = (v1 - v0) / (v1 - v0).norm();
Eigen::Vector3d t12 = (v2 - v1) / (v2 - v1).norm();
Eigen::Vector3d kb = 2.0 * t01.cross(t12)
/ (1.0 + t01.dot(t12)); //(kb)_i in bergou 2010
Eigen::Vector3d db11 = rodState_current.directors.row(/*(numSegments_rod +*/ si_rod - 1/*)
% numSegments_rod*/);
Eigen::Vector3d db21 = t01.cross(db11);
double theta1 = rodState_current.thetas[/*(numSegments_rod +*/ si_rod
- 1 /*) % numSegments_rod*/];
Eigen::Vector3d d11 = db11 * cos(theta1) + db21 * sin(theta1);
Eigen::Vector3d d21 = -db11 * sin(theta1) + db21 * cos(theta1);
Eigen::Vector3d db12 = rodState_current.directors.row(si_rod);
Eigen::Vector3d db22 = t12.cross(db12);
double theta2 = rodState_current.thetas[si_rod];
Eigen::Vector3d d12 = db12 * cos(theta2) + db22 * sin(theta2);
Eigen::Vector3d d22 = -db12 * sin(theta2) + db22 * cos(theta2);
double li_rest = 0.5
* (rod.vSegParams(/*(numSegments_rod */ +si_rod
- 1 /*) % numSegments_rod*/,
3)
+ rod.vSegParams(si_rod, 3));
double width
= 0.5
* (rod.widths[/*(numSegments_rod +*/ si_rod - 1 /*) % numSegments_rod*/]
+ rod.widths[si_rod]);
assert(width == rod.params.thickness);
//assuming rectangle cross section
const double debug_momentOfInertia_first = width
* std::pow(rod.params.thickness, 3.0)
/ 12;
const double debug_momentOfInertia_second = rod.params.thickness
* std::pow(width, 3.0) / 12;
const double debug_bendingStiffness_11 = debug_momentOfInertia_first
* rod.vSegParams(si_rod, 1);
const double debug_bendingStiffness_22 = debug_momentOfInertia_second
* rod.vSegParams(si_rod, 1);
double factor1 = debug_bendingStiffness_11 / li_rest;
double factor2 = debug_bendingStiffness_22 / li_rest;
double k1 = 0.5 * ((d21 + d22).dot(kb));
double k2 = 0.5 * ((d11 + d12).dot(kb));
double sk1 = rodState_initial.curvatures(si_rod, 0);
double sk2 = rodState_initial.curvatures(si_rod, 1);
double vertr1 = sqrt(factor1) * (k1 - sk1);
double vertr2 = sqrt(factor2) * (k2 - sk2);
r[roffset + 2 * idx] = vertr1; // bending energy Eb1
r[roffset + 2 * idx + 1] = vertr2; // bending energy Eb2
if (Jr) {
Eigen::Vector3d ttilde = (t01 + t12) / (1.0 + t01.dot(t12));
Eigen::Vector3d d1tilde = (d11 + d12) / (1.0 + t01.dot(t12));
Eigen::Vector3d d2tilde = (d21 + d22) / (1.0 + t01.dot(t12));
// Gradients for dk1/dx_(i-1) and dk1/dx_i
Eigen::Vector3d dk11 = 1.0 / (v1 - v0).norm()
* (-k1 * ttilde + t12.cross(d2tilde));
Eigen::Vector3d dk12 = 1.0 / (v2 - v1).norm()
* (-k1 * ttilde - t01.cross(d2tilde));
// const Eigen::Vector3d dEb1_dxim1 = -sqrt(factor1 * width) * dk11;
// const Eigen::Vector3d dEb1_dxi = sqrt(factor1 * width) * dk11
// - sqrt(factor1 * width) * dk12;
// const Eigen::Vector3d dEb1_dxip1 = sqrt(factor1 * width) * dk12;
// const double dE_dkappa_k_j_1 = (0.5 / li_rest) * debug_bendingStiffness_11
// * (k1 - sk1);
const Eigen::Vector3d dEb1_dxim1 = -sqrt(factor1) * dk11;
const Eigen::Vector3d dEb1_dxi = sqrt(factor1) * dk11 - sqrt(factor1) * dk12;
const Eigen::Vector3d dEb1_dxip1 = sqrt(factor1) * dk12;
// const Eigen::Vector3d debug_force1Leimer_xim1 = dEb1_dxim1 * debug_vertr1;
// const Eigen::Vector3d debug_force1Leimer_xi = dEb1_dxi * debug_vertr1;
// const Eigen::Vector3d debug_force1Leimer_xip1 = dEb1_dxip1 * debug_vertr1;
for (int j = 0; j < 3; j++) {
// Gradients for dEb1/dx_(i-1), dEb1/dx_i and dEb1/dx_(i+1)
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m0)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + 2 * idx,
3 * m0 + j,
dEb1_dxim1[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(
Eigen::Triplet<double>(roffset + 2 * idx, 3 * m1 + j, dEb1_dxi[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m2)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + 2 * idx,
3 * m2 + j,
dEb1_dxip1[j]));
#ifdef EXPLICIT_BC
}
#endif
}
// Gradients for dk2/dx_(i-1) and dk2/dx_i
Eigen::Vector3d dk21 = 1.0 / (v1 - v0).norm()
* (-k2 * ttilde + t12.cross(d1tilde));
Eigen::Vector3d dk22 = 1.0 / (v2 - v1).norm()
* (-k2 * ttilde - t01.cross(d1tilde));
// const Eigen::Vector3d dEb2_dxim1 = -sqrt(factor2 * width * width * width)
// * dk21;
// const Eigen::Vector3d dEb2_xi = sqrt(factor2 * width * width * width) * dk21
// - sqrt(factor2 * width * width * width) * dk22;
// const Eigen::Vector3d dEb2_xip1 = sqrt(factor2 * width * width * width) * dk22;
// const double dE_dkappa_k_j_2 = (0.5 / li_rest) * debug_bendingStiffness_22
// * (k2 - sk2);
const Eigen::Vector3d dEb2_dxim1 = -sqrt(factor2) * dk21;
const Eigen::Vector3d dEb2_dxi = sqrt(factor2) * dk21 - sqrt(factor2) * dk22;
const Eigen::Vector3d dEb2_dxip1 = sqrt(factor2) * dk22;
// const Eigen::Vector3d debug_force2Leimer_xim1 = dEb2_dxim1 * debug_vertr2;
// const Eigen::Vector3d debug_force2Leimer_xi = dEb2_dxi * debug_vertr2;
// const Eigen::Vector3d debug_force2Leimer_xip1 = dEb2_dxip1 * debug_vertr2;
for (int j = 0; j < 3; j++) {
// Gradients for dEb2/dx_(i-1), dEb2/dx_i and dEb2/dx_(i+1)
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m0)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + 2 * idx + 1,
3 * m0 + j,
dEb2_dxim1[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + 2 * idx + 1,
3 * m1 + j,
dEb2_dxi[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m2)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + 2 * idx + 1,
3 * m2 + j,
dEb2_dxip1[j]));
#ifdef EXPLICIT_BC
}
#endif
}
// Material curvature gradients dd/dtheta1 and dd/theta2
Eigen::Vector3d Dd11 = -db11 * sin(theta1) + db21 * cos(theta1);
Eigen::Vector3d Dd21 = -db11 * cos(theta1) - db21 * sin(theta1);
Eigen::Vector3d Dd12 = -db12 * sin(theta2) + db22 * cos(theta2);
Eigen::Vector3d Dd22 = -db12 * cos(theta2) - db22 * sin(theta2);
// Gradients for dEb1/dtheta1, dEb2/dtheta1, dEb1/dtheta2 and dEb2/dtheta2
J.push_back(
Eigen::Triplet<double>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
0.5 * sqrt(factor1 /* * width*/) * Dd21.dot(kb)));
J.push_back(
Eigen::Triplet<double>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
0.5 * sqrt(factor2 /* * width * width * width*/)
* Dd11.dot(kb)));
J.push_back(
Eigen::Triplet<double>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
0.5 * sqrt(factor1 /* * width*/) * Dd22.dot(kb)));
J.push_back(
Eigen::Triplet<double>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
0.5 * sqrt(factor2 /* * width * width * width*/)
* Dd12.dot(kb)));
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
double chi = 1 + t01.dot(t12);
double len01 = (v1 - v0).norm();
double len12 = (v2 - v1).norm();
double d2k1_dtheta1theta1 = -kb.dot(0.5 * d21);
double d2k1_dtheta2theta2 = -kb.dot(0.5 * d22);
double d2k2_dtheta1theta1 = -kb.dot(0.5 * d11);
double d2k2_dtheta2theta2 = -kb.dot(0.5 * d12);
Eigen::Vector3d d2k1_de01theta1 = 1 / chi * (-t12 / len01).cross(d11)
+ (ttilde / len01) * kb.dot(0.5 * d11);
Eigen::Vector3d d2k1_de01theta2 = 1 / chi * (-t12 / len01).cross(d12)
+ (ttilde / len01) * kb.dot(0.5 * d12);
Eigen::Vector3d d2k1_de12theta1 = 1 / chi * (t01 / len12).cross(d11)
+ (ttilde / len12) * kb.dot(0.5 * d11);
Eigen::Vector3d d2k1_de12theta2 = 1 / chi * (t01 / len12).cross(d12)
+ (ttilde / len12) * kb.dot(0.5 * d12);
Eigen::Vector3d d2k2_de01theta1 = -1 / chi * (-t12 / len01).cross(d21)
- (ttilde / len01) * kb.dot(0.5 * d21);
Eigen::Vector3d d2k2_de01theta2 = -1 / chi * (-t12 / len01).cross(d22)
- (ttilde / len01) * kb.dot(0.5 * d22);
Eigen::Vector3d d2k2_de12theta1 = -1 / chi * (t01 / len12).cross(d21)
- (ttilde / len12) * kb.dot(0.5 * d21);
Eigen::Vector3d d2k2_de12theta2 = -1 / chi * (t01 / len12).cross(d22)
- (ttilde / len12) * kb.dot(0.5 * d22);
// curvature for infinitesimal transport
Eigen::Matrix3d d2k1_de01e01
= 1 / (len01 * len01)
* (2 * k1 * ttilde * ttilde.transpose()
- t12.cross(d2tilde) * ttilde.transpose()
- ttilde * t12.cross(d2tilde).transpose()
+ 0.5 * kb * d21.transpose()
- k1 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
- t01 * t01.transpose()));
Eigen::Matrix3d d2k1_de12e12
= 1 / (len12 * len12)
* (2 * k1 * ttilde * ttilde.transpose()
+ t01.cross(d2tilde) * ttilde.transpose()
+ ttilde * t01.cross(d2tilde).transpose()
+ 0.5 * kb * d22.transpose()
- k1 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
- t12 * t12.transpose()));
Eigen::Matrix3d d2k1_de01e12
= 1 / (len01 * len12)
* (2 * k1 * ttilde * ttilde.transpose()
- t12.cross(d2tilde) * ttilde.transpose()
+ ttilde * t01.cross(d2tilde).transpose() - skewSym(d2tilde)
- k1 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
+ t01 * t12.transpose()));
Eigen::Matrix3d d2k1_de12e01
= 1 / (len01 * len12)
* (2 * k1 * ttilde * ttilde.transpose()
+ t01.cross(d2tilde) * ttilde.transpose()
- ttilde * t12.cross(d2tilde).transpose() + skewSym(d2tilde)
- k1 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
+ t12 * t01.transpose()));
Eigen::Matrix3d d2k2_de01e01
= 1 / (len01 * len01)
* (2 * k2 * ttilde * ttilde.transpose()
- t12.cross(d1tilde) * ttilde.transpose()
- ttilde * t12.cross(d1tilde).transpose()
+ 0.5 * kb * d11.transpose()
- k2 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
- t01 * t01.transpose()));
Eigen::Matrix3d d2k2_de12e12
= 1 / (len12 * len12)
* (2 * k2 * ttilde * ttilde.transpose()
+ t01.cross(d1tilde) * ttilde.transpose()
+ ttilde * t01.cross(d1tilde).transpose()
+ 0.5 * kb * d12.transpose()
- k2 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
- t12 * t12.transpose()));
Eigen::Matrix3d d2k2_de01e12
= 1 / (len01 * len12)
* (2 * k2 * ttilde * ttilde.transpose()
- t12.cross(d1tilde) * ttilde.transpose()
+ ttilde * t01.cross(d1tilde).transpose() - skewSym(d1tilde)
- k2 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
+ t01 * t12.transpose()));
Eigen::Matrix3d d2k2_de12e01
= 1 / (len01 * len12)
* (2 * k2 * ttilde * ttilde.transpose()
+ t01.cross(d1tilde) * ttilde.transpose()
- ttilde * t12.cross(d1tilde).transpose() + skewSym(d1tilde)
- k2 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
+ t12 * t01.transpose()));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(sqrt(factor1 * width) * d2k1_dtheta1theta1);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_dtheta1theta1);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
dofoffset + thetaoffset + si_rod));
H_value->push_back(sqrt(factor1 * width) * d2k1_dtheta2theta2);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
dofoffset + thetaoffset + si_rod));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_dtheta2theta2);
double df1_dkb = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.widths[si_rod])
/ (4 * li_rest * sqrt(factor1 * rod.widths[si_rod]));
double df1_dw = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.vSegParams(si_rod, 1))
/ (4 * li_rest * sqrt(factor1 * rod.widths[si_rod]));
double df1_dt = (3 * rod.params.thickness * rod.params.thickness
* rod.vSegParams(si_rod, 1) * rod.widths[si_rod])
/ (4 * li_rest * sqrt(factor1 * rod.widths[si_rod]));
double df2_dkb = (rod.params.thickness * rod.widths[si_rod]
* rod.widths[si_rod] * rod.widths[si_rod])
/ (4 * li_rest
* sqrt(factor2 * rod.widths[si_rod] * rod.widths[si_rod]
* rod.widths[si_rod]));
double df2_dw = (3 * rod.params.thickness * rod.vSegParams(si_rod, 1)
* rod.widths[si_rod] * rod.widths[si_rod])
/ (4 * li_rest
* sqrt(factor2 * rod.widths[si_rod] * rod.widths[si_rod]
* rod.widths[si_rod]));
double df2_dt = (rod.vSegParams(si_rod, 1) * rod.widths[si_rod]
* rod.widths[si_rod] * rod.widths[si_rod])
/ (4 * li_rest
* sqrt(factor2 * rod.widths[si_rod] * rod.widths[si_rod]
* rod.widths[si_rod]));
Jui.push_back(Eigen::Triplet<double>(roffset + 2 * idx,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod,
df1_dkb * (k1 - sk1)));
Jui.push_back(Eigen::Triplet<double>(roffset + 2 * idx,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod,
df1_dw * (k1 - sk1)));
Jui.push_back(Eigen::Triplet<double>(roffset + 2 * idx,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod,
df1_dt * (k1 - sk1)));
Jui.push_back(Eigen::Triplet<double>(roffset + 2 * idx + 1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod,
df2_dkb * (k2 - sk2)));
Jui.push_back(Eigen::Triplet<double>(roffset + 2 * idx + 1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod,
df2_dw * (k2 - sk2)));
Jui.push_back(Eigen::Triplet<double>(roffset + 2 * idx + 1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod,
df2_dt * (k2 - sk2)));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df1_dkb * Dd21.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df2_dkb * Dd11.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df1_dkb * Dd22.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df2_dkb * Dd12.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df1_dw * Dd21.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df2_dw * Dd11.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df1_dw * Dd22.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df2_dw * Dd12.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df1_dt * Dd21.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df2_dt * Dd11.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df1_dt * Dd22.dot(kb));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(0.5 * df2_dt * Dd12.dot(kb));
for (int h1 = 0; h1 < 3; h1++) {
// d2k1/de01theta1 and d2k1/de01theta2 for vertices
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(sqrt(factor1 * width) * d2k1_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m0 + h1));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m1 + h1));
H_value->push_back(sqrt(factor1 * width) * d2k1_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de01theta2[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(sqrt(factor1 * width) * d2k1_de01theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
3 * m0 + h1));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de01theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
3 * m1 + h1));
H_value->push_back(sqrt(factor1 * width) * d2k1_de01theta2[h1]);
// d2k1/de12theta1 and d2k1/de12theta2 for vertices
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(sqrt(factor1 * width) * d2k1_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m1 + h1));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m2 + h1));
H_value->push_back(sqrt(factor1 * width) * d2k1_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de12theta2[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(sqrt(factor1 * width) * d2k1_de12theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
3 * m1 + h1));
H_value->push_back(-sqrt(factor1 * width) * d2k1_de12theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
dofoffset + thetaoffset + si_rod,
3 * m2 + h1));
H_value->push_back(sqrt(factor1 * width) * d2k1_de12theta2[h1]);
// d2k2/de01theta1 and d2k2/de01theta2 for vertices
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m0 + h1));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m1 + h1));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de01theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de01theta2[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de01theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
3 * m0 + h1));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de01theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
3 * m1 + h1));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de01theta2[h1]);
// d2k2/de12theta1 and d2k2/de12theta2 for vertices
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m1 + h1));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
3 * m2 + h1));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de12theta1[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de12theta2[h1]);
H_index->push_back(
Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
dofoffset + thetaoffset + si_rod));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de12theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
3 * m1 + h1));
H_value->push_back(-sqrt(factor2 * width * width * width)
* d2k2_de12theta2[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
dofoffset + thetaoffset + si_rod,
3 * m2 + h1));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de12theta2[h1]);
for (int h2 = 0; h2 < 3; h2++) {
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
3 * m0 + h2));
H_value->push_back(sqrt(factor1 * width) * d2k1_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
3 * m1 + h2));
H_value->push_back(sqrt(factor1 * width)
* (-d2k1_de01e01(h1, h2) + d2k1_de01e12(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
3 * m2 + h2));
H_value->push_back(sqrt(factor1 * width) * (-d2k1_de01e12(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
3 * m2 + h2));
H_value->push_back(sqrt(factor1 * width) * d2k1_de12e12(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
3 * m1 + h2));
H_value->push_back(sqrt(factor1 * width)
* (-d2k1_de12e12(h1, h2) + d2k1_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
3 * m0 + h2));
H_value->push_back(sqrt(factor1 * width) * (-d2k1_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
3 * m1 + h2));
H_value->push_back(sqrt(factor1 * width)
* (d2k1_de01e01(h1, h2) - d2k1_de01e12(h1, h2)
+ d2k1_de12e12(h1, h2)
- d2k1_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
3 * m0 + h2));
H_value->push_back(sqrt(factor1 * width)
* (-d2k1_de01e01(h1, h2) + d2k1_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
3 * m2 + h2));
H_value->push_back(sqrt(factor1 * width)
* (d2k1_de01e12(h1, h2) - d2k1_de12e12(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
3 * m0 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
3 * m1 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (-d2k2_de01e01(h1, h2) + d2k2_de01e12(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
3 * m2 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (-d2k2_de01e12(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
3 * m2 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* d2k2_de12e12(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
3 * m1 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (-d2k2_de12e12(h1, h2) + d2k2_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
3 * m0 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (-d2k2_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
3 * m1 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (d2k2_de01e01(h1, h2) - d2k2_de01e12(h1, h2)
+ d2k2_de12e12(h1, h2)
- d2k2_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
3 * m0 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (-d2k2_de01e01(h1, h2) + d2k2_de12e01(h1, h2)));
H_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
3 * m2 + h2));
H_value->push_back(sqrt(factor2 * width * width * width)
* (d2k2_de01e12(h1, h2) - d2k2_de12e12(h1, h2)));
}
// stiffness derivatives
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dkb * -dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dkb * (dk11[h1] - dk12[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dkb * dk12[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dkb * -dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dkb * (dk21[h1] - dk22[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
uistiffoffset + 1 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dkb * dk22[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dw * -dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dw * (dk11[h1] - dk12[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dw * dk12[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dw * -dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dw * (dk21[h1] - dk22[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dw * dk22[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m0 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dt * -dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m1 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dt * (dk11[h1] - dk12[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx,
3 * m2 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df1_dt * dk12[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m0 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dt * -dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m1 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dt * (dk21[h1] - dk22[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + 2 * idx + 1,
3 * m2 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df2_dt * dk22[h1]);
}
constexpr bool numericalHessian = false;
if (numericalHessian) {
double gradInc = 1e-6;
Eigen::Matrix3d num_d2k1_de01e01, num_d2k1_de01e12, num_d2k1_de12e01,
num_d2k1_de12e12;
Eigen::Matrix3d num_d2k2_de01e01, num_d2k2_de01e12, num_d2k2_de12e01,
num_d2k2_de12e12;
Eigen::Vector3d num_dk2_e01;
Eigen::Vector3d num_d2k1_de01theta1, num_d2k1_de01theta2,
num_d2k1_de12theta1, num_d2k1_de12theta2;
Eigen::Vector3d num_d2k2_de01theta1, num_d2k2_de01theta2,
num_d2k2_de12theta1, num_d2k2_de12theta2;
double num_d2k1_dtheta1theta1, num_d2k1_dtheta2theta2,
num_d2k2_dtheta1theta1, num_d2k2_dtheta2theta2;
for (int j = 0; j < 3; j++) {
Eigen::Vector3d gradIncVec(0, 0, 0);
gradIncVec[j] += gradInc;
Eigen::Vector3d _v0 = rodState_current.centerlinePositions
.row((numVertices_rod + si_rod - 1)
% numVertices_rod)
.transpose()
+ gradIncVec;
Eigen::Vector3d _v1
= rodState_current.centerlinePositions.row(si_rod).transpose()
+ gradIncVec;
Eigen::Vector3d _v2 = rodState_current.centerlinePositions
.row((si_rod + 1) % numVertices_rod)
.transpose()
+ gradIncVec;
double _theta1 = theta1 + gradInc;
double _theta2 = theta2 + gradInc;
Eigen::Vector3d t01_v0 = (v1 - _v0) / (v1 - _v0).norm();
Eigen::Vector3d t01_v1 = (_v1 - v0) / (_v1 - v0).norm();
Eigen::Vector3d t12_v1 = (v2 - _v1) / (v2 - _v1).norm();
Eigen::Vector3d t12_v2 = (_v2 - v1) / (_v2 - v1).norm();
Eigen::Vector3d kb_v0 = 2.0 * t01_v0.cross(t12)
/ (1.0 + t01_v0.dot(t12));
Eigen::Vector3d kb_v1 = 2.0 * t01_v1.cross(t12_v1)
/ (1.0 + t01_v1.dot(t12_v1));
Eigen::Vector3d kb_v2 = 2.0 * t01.cross(t12_v2)
/ (1.0 + t01.dot(t12_v2));
Eigen::Vector3d db11_v0
= parallelTransport(rodState_current.directors.row(
(numSegments_rod + si_rod - 1)
% numSegments_rod),
t01,
t01_v0);
Eigen::Vector3d db11_v1
= parallelTransport(rodState_current.directors.row(
(numSegments_rod + si_rod - 1)
% numSegments_rod),
t01,
t01_v1);
Eigen::Vector3d db12_v1
= parallelTransport(rodState_current.directors.row(si_rod),
t12,
t12_v1);
Eigen::Vector3d db12_v2
= parallelTransport(rodState_current.directors.row(si_rod),
t12,
t12_v2);
Eigen::Vector3d db21_v0 = t01_v0.cross(db11_v0);
Eigen::Vector3d db21_v1 = t01_v1.cross(db11_v1);
Eigen::Vector3d db22_v1 = t12_v1.cross(db12_v1);
Eigen::Vector3d db22_v2 = t12_v2.cross(db12_v2);
Eigen::Vector3d d11_v0 = db11_v0 * cos(theta1)
+ db21_v0 * sin(theta1);
Eigen::Vector3d d11_v1 = db11_v1 * cos(theta1)
+ db21_v1 * sin(theta1);
Eigen::Vector3d d21_v0 = -db11_v0 * sin(theta1)
+ db21_v0 * cos(theta1);
Eigen::Vector3d d21_v1 = -db11_v1 * sin(theta1)
+ db21_v1 * cos(theta1);
Eigen::Vector3d d12_v1 = db12_v1 * cos(theta2)
+ db22_v1 * sin(theta2);
Eigen::Vector3d d12_v2 = db12_v2 * cos(theta2)
+ db22_v2 * sin(theta2);
Eigen::Vector3d d22_v1 = -db12_v1 * sin(theta2)
+ db22_v1 * cos(theta2);
Eigen::Vector3d d22_v2 = -db12_v2 * sin(theta2)
+ db22_v2 * cos(theta2);
Eigen::Vector3d d11_theta1 = db11 * cos(_theta1)
+ db21 * sin(_theta1);
Eigen::Vector3d d21_theta1 = -db11 * sin(_theta1)
+ db21 * cos(_theta1);
Eigen::Vector3d d12_theta2 = db12 * cos(_theta2)
+ db22 * sin(_theta2);
Eigen::Vector3d d22_theta2 = -db12 * sin(_theta2)
+ db22 * cos(_theta2);
double k1_v0 = 0.5 * ((d21_v0 + d22).dot(kb_v0));
double k1_v1 = 0.5 * ((d21_v1 + d22_v1).dot(kb_v1));
double k1_v2 = 0.5 * ((d21 + d22_v2).dot(kb_v2));
double k2_v0 = 0.5 * ((d11_v0 + d12).dot(kb_v0));
double k2_v1 = 0.5 * ((d11_v1 + d12_v1).dot(kb_v1));
double k2_v2 = 0.5 * ((d11 + d12_v2).dot(kb_v2));
double k1_theta1 = 0.5 * ((d21_theta1 + d22).dot(kb));
double k1_theta2 = 0.5 * ((d21 + d22_theta2).dot(kb));
double k2_theta1 = 0.5 * ((d11_theta1 + d12).dot(kb));
double k2_theta2 = 0.5 * ((d11 + d12_theta2).dot(kb));
num_d2k1_de01e01.col(j)
= (1.0 / (v1 - _v0).norm()
* (-k1_v0 * (t01_v0 + t12) / (1.0 + t01_v0.dot(t12))
+ t12.cross((d21_v0 + d22) / (1.0 + t01_v0.dot(t12))))
- dk11)
/ gradInc;
num_d2k1_de01e12.col(j)
= (1.0 / (v1 - v0).norm()
* (-k1_v2 * (t01 + t12_v2) / (1.0 + t01.dot(t12_v2))
+ t12_v2.cross((d21 + d22_v2)
/ (1.0 + t01.dot(t12_v2))))
- dk11)
/ gradInc;
num_d2k1_de12e01.col(j)
= (1.0 / (v2 - v1).norm()
* (-k1_v0 * (t01_v0 + t12) / (1.0 + t01_v0.dot(t12))
- t01_v0.cross((d21_v0 + d22)
/ (1.0 + t01_v0.dot(t12))))
- dk12)
/ gradInc;
num_d2k1_de12e12.col(j)
= (1.0 / (_v2 - v1).norm()
* (-k1_v2 * (t01 + t12_v2) / (1.0 + t01.dot(t12_v2))
- t01.cross((d21 + d22_v2) / (1.0 + t01.dot(t12_v2))))
- dk12)
/ gradInc;
num_d2k2_de01e01.col(j)
= (1.0 / (v1 - _v0).norm()
* (-k2_v0 * (t01_v0 + t12) / (1.0 + t01_v0.dot(t12))
+ t12.cross((d11_v0 + d12) / (1.0 + t01_v0.dot(t12))))
- dk21)
/ gradInc;
num_d2k2_de01e12.col(j)
= (1.0 / (v1 - v0).norm()
* (-k2_v2 * (t01 + t12_v2) / (1.0 + t01.dot(t12_v2))
+ t12_v2.cross((d11 + d12_v2)
/ (1.0 + t01.dot(t12_v2))))
- dk21)
/ gradInc;
num_d2k2_de12e01.col(j)
= (1.0 / (v2 - v1).norm()
* (-k2_v0 * (t01_v0 + t12) / (1.0 + t01_v0.dot(t12))
- t01_v0.cross((d11_v0 + d12)
/ (1.0 + t01_v0.dot(t12))))
- dk22)
/ gradInc;
num_d2k2_de12e12.col(j)
= (1.0 / (_v2 - v1).norm()
* (-k2_v2 * (t01 + t12_v2) / (1.0 + t01.dot(t12_v2))
- t01.cross((d11 + d12_v2) / (1.0 + t01.dot(t12_v2))))
- dk22)
/ gradInc;
num_d2k1_de01theta1 = (1.0 / (v1 - v0).norm()
* (-k1_theta1 * (t01 + t12)
/ (1.0 + t01.dot(t12))
+ t12.cross((d21_theta1 + d22)
/ (1.0 + t01.dot(t12))))
- dk11)
/ gradInc;
num_d2k1_de01theta2 = (1.0 / (v1 - v0).norm()
* (-k1_theta2 * (t01 + t12)
/ (1.0 + t01.dot(t12))
+ t12.cross((d21 + d22_theta2)
/ (1.0 + t01.dot(t12))))
- dk11)
/ gradInc;
num_d2k1_de12theta1 = (1.0 / (v2 - v1).norm()
* (-k1_theta1 * (t01 + t12)
/ (1.0 + t01.dot(t12))
- t01.cross((d21_theta1 + d22)
/ (1.0 + t01.dot(t12))))
- dk12)
/ gradInc;
num_d2k1_de12theta2 = (1.0 / (v2 - v1).norm()
* (-k1_theta2 * (t01 + t12)
/ (1.0 + t01.dot(t12))
- t01.cross((d21 + d22_theta2)
/ (1.0 + t01.dot(t12))))
- dk12)
/ gradInc;
num_d2k2_de01theta1 = (1.0 / (v1 - v0).norm()
* (-k2_theta1 * (t01 + t12)
/ (1.0 + t01.dot(t12))
+ t12.cross((d11_theta1 + d12)
/ (1.0 + t01.dot(t12))))
- dk21)
/ gradInc;
num_d2k2_de01theta2 = (1.0 / (v1 - v0).norm()
* (-k2_theta2 * (t01 + t12)
/ (1.0 + t01.dot(t12))
+ t12.cross((d11 + d12_theta2)
/ (1.0 + t01.dot(t12))))
- dk21)
/ gradInc;
num_d2k2_de12theta1 = (1.0 / (v2 - v1).norm()
* (-k2_theta1 * (t01 + t12)
/ (1.0 + t01.dot(t12))
- t01.cross((d11_theta1 + d12)
/ (1.0 + t01.dot(t12))))
- dk22)
/ gradInc;
num_d2k2_de12theta2 = (1.0 / (v2 - v1).norm()
* (-k2_theta2 * (t01 + t12)
/ (1.0 + t01.dot(t12))
- t01.cross((d11 + d12_theta2)
/ (1.0 + t01.dot(t12))))
- dk22)
/ gradInc;
num_d2k1_dtheta1theta1
= (0.5 * (-db11 * cos(_theta1) - db21 * sin(_theta1)).dot(kb)
- 0.5 * Dd21.dot(kb))
/ gradInc;
num_d2k2_dtheta1theta1
= (0.5 * (-db11 * sin(_theta1) + db21 * cos(_theta1)).dot(kb)
- 0.5 * Dd11.dot(kb))
/ gradInc;
num_d2k1_dtheta2theta2
= (0.5 * (-db12 * cos(_theta2) - db22 * sin(_theta2)).dot(kb)
- 0.5 * Dd22.dot(kb))
/ gradInc;
num_d2k2_dtheta2theta2
= (0.5 * (-db12 * sin(_theta2) + db22 * cos(_theta2)).dot(kb)
- 0.5 * Dd12.dot(kb))
/ gradInc;
}
int stop = 0;
}
}
#endif
}
} else {
r[roffset + 2 * idx] = 0;
r[roffset + 2 * idx + 1] = 0;
}
idx++;
}
roffset += 2 * idx;
// twisting terms
idx = 0;
for (int si_rod = startseg; si_rod < numSegments_rod; si_rod++) {
int m0 = rodState_current
.centerlineIndices[(numVertices_rod + si_rod - 1) % numVertices_rod];
int m1 = rodState_current.centerlineIndices[si_rod];
int m2 = rodState_current.centerlineIndices[(si_rod + 1) % numVertices_rod];
if (indexOfIntersection[m1] < 0) {
Eigen::Vector3d v0 = rodState_current.centerlinePositions
.row((numVertices_rod + si_rod - 1) % numVertices_rod)
.transpose();
Eigen::Vector3d v1 = rodState_current.centerlinePositions.row(si_rod).transpose();
Eigen::Vector3d v2 = rodState_current.centerlinePositions
.row((si_rod + 1) % numVertices_rod)
.transpose();
Eigen::Vector3d t01 = (v1 - v0) / (v1 - v0).norm();
Eigen::Vector3d t12 = (v2 - v1) / (v2 - v1).norm();
Eigen::Vector3d kb = 2.0 * t01.cross(t12) / (1.0 + t01.dot(t12));
Eigen::Vector3d db11 = rodState_current.directors.row((numSegments_rod + si_rod - 1)
% numSegments_rod);
Eigen::Vector3d db21 = t01.cross(db11);
Eigen::Vector3d db12 = rodState_current.directors.row(si_rod);
Eigen::Vector3d db22 = t12.cross(db12);
double theta1 = rodState_current
.thetas[(numSegments_rod + si_rod - 1) % numSegments_rod];
double theta2 = rodState_current.thetas[si_rod];
Eigen::Vector3d d1 = db11 * cos(theta1) + db21 * sin(theta1);
Eigen::Vector3d d2 = db12 * cos(theta2) + db22 * sin(theta2);
Eigen::Vector3d d1t = parallelTransport(d1, v1 - v0, v2 - v1);
double theta = angle(
d1t,
d2,
t12); // angle difference between vector of second segment and parallel-transported vector of first segment
double len = 0.5
* (rod.vSegParams((numSegments_rod + si_rod - 1) % numSegments_rod, 3)
+ rod.vSegParams(si_rod, 3));
double width = 0.5
* (rod.widths[(numSegments_rod + si_rod - 1) % numSegments_rod]
+ rod.widths[si_rod]);
double factor = 0.5 * rod.vSegParams(si_rod, 2) * rod.params.thickness
* rod.params.thickness * rod.params.thickness / len;
assert(rod.params.thickness == width);
const double G = rod.vSegParams(si_rod, 2);
const double torsionalConstant_J = 0.141 * std::pow(rod.params.thickness, 4);
const double debug_factor = /*0.5 **/ torsionalConstant_J * G / len;
// r[roffset + idx] = sqrt(factor * width) * theta; // twisting energy Et
r[roffset + idx] = sqrt(debug_factor) * theta; // twisting energy Et
const double debug_sqrd_r = 0.5 * r[roffset + idx] * r[roffset + idx];
if (Jr) {
// Gradient dtheta1/dx and dtheta2/dx
Eigen::Vector3d dtheta1 = 0.5 * kb / (v1 - v0).norm();
Eigen::Vector3d dtheta2 = 0.5 * kb / (v2 - v1).norm();
const Eigen::Vector3d jacob_theta1 = sqrt(debug_factor) * dtheta1;
const Eigen::Vector3d debug_leimerTwistForce_pos1 = jacob_theta1
* r[roffset + idx];
const Eigen::Vector3d jacob_theta2 = sqrt(debug_factor) * dtheta2;
const Eigen::Vector3d debug_leimerTwistForce_pos2 = jacob_theta2
* r[roffset + idx];
for (int j = 0; j < 3; j++) {
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m0)) {
#endif
J.push_back(
Eigen::Triplet<double>(roffset + idx, 3 * m0 + j, -jacob_theta1[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + idx,
3 * m1 + j,
jacob_theta1[j] - jacob_theta2[j]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m2)) {
#endif
J.push_back(
Eigen::Triplet<double>(roffset + idx, 3 * m2 + j, jacob_theta2[j]));
#ifdef EXPLICIT_BC
}
#endif
}
// Gradients dEt/dtheta1 and dEt/dtheta2
const double debug_leimerTwistForce_theta1 = sqrt(debug_factor)
* r[roffset + idx];
J.push_back(Eigen::Triplet<double>(roffset + idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
-sqrt(debug_factor)));
const double debug_leimerTwistForce_theta2 = -sqrt(debug_factor)
* r[roffset + idx];
J.push_back(Eigen::Triplet<double>(roffset + idx,
dofoffset + thetaoffset + si_rod,
sqrt(debug_factor)));
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
double chi = 1 + t01.dot(t12);
double len01 = (v1 - v0).norm();
double len12 = (v2 - v1).norm();
// twist for infinitesimal transport
Eigen::Matrix3d d2theta_d01d01
= 1 / (2 * len01 * len01)
* (-2 * skewSym(t12) / chi
- kb * ((t01 + t12) / chi + t01).transpose());
Eigen::Matrix3d d2theta_d12d12
= 1 / (2 * len12 * len12)
* (2 * skewSym(t01) / chi
- kb * ((t01 + t12) / chi + t12).transpose());
Eigen::Matrix3d d2theta_d12d01 = 1 / (2 * len12 * len01)
* (-2 * skewSym(t12) / chi
- kb * ((t01 + t12) / chi).transpose());
Eigen::Matrix3d d2theta_d01d12 = 1 / (2 * len01 * len12)
* (2 * skewSym(t01) / chi
- kb * ((t01 + t12) / chi).transpose());
//// twist for finite transport
//Eigen::Matrix3d d2theta_d01d01 = 1 / (4 * len01*len01) * (kb * (t01 + (t01 + t12) / chi).transpose() + (t01 + (t01 + t12) / chi) * kb.transpose());
//Eigen::Matrix3d d2theta_d12d12 = 1 / (4 * len12*len12) * (kb * (t12 + (t01 + t12) / chi).transpose() + (t12 + (t01 + t12) / chi) * kb.transpose());
//Eigen::Matrix3d d2theta_d01d12 = 1 / (2 * len01*len12) * (2 * skewSym(t01) / chi - kb * ((t01 + t12) / chi).transpose());
//Eigen::Matrix3d d2theta_d12d01 = d2theta_d01d12.transpose();
double df_dkt = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.widths[si_rod])
/ (4 * len * sqrt(factor * rod.widths[si_rod]));
double df_dw = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.vSegParams(si_rod, 2))
/ (4 * len * sqrt(factor * rod.widths[si_rod]));
double df_dt = (3 * rod.params.thickness * rod.params.thickness
* rod.vSegParams(si_rod, 2) * rod.widths[si_rod])
/ (4 * len * sqrt(factor * rod.widths[si_rod]));
Jui.push_back(Eigen::Triplet<double>(roffset + idx,
uistiffoffset + 2 * nsegsTotal
+ dofoffset + si_rod,
df_dkt * theta));
Jui.push_back(Eigen::Triplet<double>(roffset + idx,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod,
df_dw * theta));
Jui.push_back(Eigen::Triplet<double>(roffset + idx,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod,
df_dt * theta));
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 2 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(-df_dkt);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 2 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dkt);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(-df_dw);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dw);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
dofoffset + thetaoffset
+ (numSegments_rod + si_rod - 1)
% numSegments_rod,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(-df_dt);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
dofoffset + thetaoffset + si_rod,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dt);
for (int h1 = 0; h1 < 3; h1++) {
for (int h2 = 0; h2 < 3; h2++) {
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m0 + h1, 3 * m0 + h2));
H_value->push_back(sqrt(factor * width) * d2theta_d01d01(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m0 + h1, 3 * m1 + h2));
H_value->push_back(
sqrt(factor * width)
* (-d2theta_d01d01(h1, h2) + d2theta_d01d12(h1, h2)));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m0 + h1, 3 * m2 + h2));
H_value->push_back(sqrt(factor * width) * (-d2theta_d01d12(h1, h2)));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m2 + h1, 3 * m2 + h2));
H_value->push_back(sqrt(factor * width) * d2theta_d12d12(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m2 + h1, 3 * m1 + h2));
H_value->push_back(
sqrt(factor * width)
* (-d2theta_d12d12(h1, h2) + d2theta_d12d01(h1, h2)));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m2 + h1, 3 * m0 + h2));
H_value->push_back(sqrt(factor * width) * (-d2theta_d12d01(h1, h2)));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m1 + h1, 3 * m1 + h2));
H_value->push_back(
sqrt(factor * width)
* (d2theta_d01d01(h1, h2) - d2theta_d01d12(h1, h2)
+ d2theta_d12d12(h1, h2) - d2theta_d12d01(h1, h2)));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m1 + h1, 3 * m0 + h2));
H_value->push_back(
sqrt(factor * width)
* (-d2theta_d01d01(h1, h2) + d2theta_d12d01(h1, h2)));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx, 3 * m1 + h1, 3 * m2 + h2));
H_value->push_back(
sqrt(factor * width)
* (d2theta_d01d12(h1, h2) - d2theta_d12d12(h1, h2)));
}
// stiffness derivatives
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m0 + h1,
uistiffoffset + 2 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dkt * -dtheta1[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m1 + h1,
uistiffoffset + 2 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dkt * (dtheta1[h1] - dtheta2[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m2 + h1,
uistiffoffset + 2 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dkt * dtheta2[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m0 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dw * -dtheta1[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m1 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dw * (dtheta1[h1] - dtheta2[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m2 + h1,
uistiffoffset + 3 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dw * dtheta2[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m0 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dt * -dtheta1[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m1 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dt * (dtheta1[h1] - dtheta2[h1]));
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx,
3 * m2 + h1,
uistiffoffset + 4 * nsegsTotal
+ dofoffset + si_rod));
Hu_value->push_back(df_dt * dtheta2[h1]);
}
constexpr bool numericalHessian = false;
if (numericalHessian) {
double gradInc = 1e-6;
Eigen::Matrix3d num_d2theta_d01d01, num_d2theta_d01d12,
num_d2theta_d12d01, num_d2theta_d12d12;
Eigen::Vector3d num_d2theta_d01dtheta1, num_d2theta_d01dtheta2,
num_d2theta_d12dtheta1, num_d2theta_d12dtheta2;
for (int j = 0; j < 3; j++) {
Eigen::Vector3d gradIncVec(0, 0, 0);
gradIncVec[j] += gradInc;
Eigen::Vector3d _v0 = rodState_current.centerlinePositions
.row((numVertices_rod + si_rod - 1)
% numVertices_rod)
.transpose()
+ gradIncVec;
Eigen::Vector3d _v1
= rodState_current.centerlinePositions.row(si_rod).transpose()
+ gradIncVec;
Eigen::Vector3d _v2 = rodState_current.centerlinePositions
.row((si_rod + 1) % numVertices_rod)
.transpose()
+ gradIncVec;
Eigen::Vector3d t01_v0 = (v1 - _v0) / (v1 - _v0).norm();
Eigen::Vector3d t01_v1 = (_v1 - v0) / (_v1 - v0).norm();
Eigen::Vector3d t12_v1 = (v2 - _v1) / (v2 - _v1).norm();
Eigen::Vector3d t12_v2 = (_v2 - v1) / (_v2 - v1).norm();
Eigen::Vector3d kb_v0 = 2.0 * t01_v0.cross(t12)
/ (1.0 + t01_v0.dot(t12));
Eigen::Vector3d kb_v1 = 2.0 * t01_v1.cross(t12_v1)
/ (1.0 + t01_v1.dot(t12_v1));
Eigen::Vector3d kb_v2 = 2.0 * t01.cross(t12_v2)
/ (1.0 + t01.dot(t12_v2));
Eigen::Vector3d db11_v0
= parallelTransport(rodState_current.directors.row(
(numSegments_rod + si_rod - 1)
% numSegments_rod),
t01,
t01_v0);
Eigen::Vector3d db11_v1
= parallelTransport(rodState_current.directors.row(
(numSegments_rod + si_rod - 1)
% numSegments_rod),
t01,
t01_v1);
Eigen::Vector3d db12_v1
= parallelTransport(rodState_current.directors.row(si_rod),
t12,
t12_v1);
Eigen::Vector3d db12_v2
= parallelTransport(rodState_current.directors.row(si_rod),
t12,
t12_v2);
Eigen::Vector3d db21_v0 = t01_v0.cross(db11_v0);
Eigen::Vector3d db21_v1 = t01_v1.cross(db11_v1);
Eigen::Vector3d db22_v1 = t12_v1.cross(db12_v1);
Eigen::Vector3d db22_v2 = t12_v2.cross(db12_v2);
Eigen::Vector3d d1_v0 = db11_v0 * cos(theta1)
+ db21_v0 * sin(theta1);
Eigen::Vector3d d1_v1 = db11_v1 * cos(theta1)
+ db21_v1 * sin(theta1);
Eigen::Vector3d d2_v1 = db12_v1 * cos(theta2)
+ db22_v1 * sin(theta2);
Eigen::Vector3d d2_v2 = db12_v2 * cos(theta2)
+ db22_v2 * sin(theta2);
Eigen::Vector3d d1t_v0 = parallelTransport(d1_v0, v1 - _v0, v2 - v1);
Eigen::Vector3d d1t_v1 = parallelTransport(d1_v1,
_v1 - v0,
v2 - _v1);
Eigen::Vector3d d1t_v2 = parallelTransport(d1, v1 - v0, _v2 - v1);
double theta_v0 = angle(d1t_v0, d2, t12);
double theta_v1 = angle(d1t_v1, d2_v1, t12_v1);
double theta_v2 = angle(d1t_v2, d2_v2, t12_v2);
num_d2theta_d01d01.col(j) = (0.5 * kb_v0 / (v1 - _v0).norm()
- dtheta1)
/ gradInc;
num_d2theta_d01d12.col(j) = (0.5 * kb_v2 / (v1 - v0).norm()
- dtheta1)
/ gradInc;
num_d2theta_d12d12.col(j) = (0.5 * kb_v2 / (_v2 - v1).norm()
- dtheta2)
/ gradInc;
num_d2theta_d12d01.col(j) = (0.5 * kb_v0 / (v2 - v1).norm()
- dtheta2)
/ gradInc;
}
int stop = 0;
}
}
#endif
}
} else
r[roffset + idx] = 0;
idx++;
}
roffset += idx;
dofoffset += rod.numSegments();
}
#ifndef EXPLICIT_BC
// mesh sliding energy Eslide
// allows ribbons to slide tangentially on mesh surface they try to approximate
// see paper Section 7.1
// Note: like gravity, this is currently applied once per rod for shared vertices
if (params.anchorPoints) {
int idx = 0;
int dofoffset = 0;
for (int rodidx = 0; rodidx < config.numRods(); rodidx++) {
Rod *rod = config.rods[rodidx];
double normalStiffness = 1e3;
double tangentStiffness = 1e0;
int nverts = rod->numVertices();
for (int i = 0; i < nverts; i++) {
Eigen::Vector3d disp = rod->curState.centerlinePositions.row(i).transpose()
- params.anchorPoints->row(idx / 3).transpose();
Eigen::Vector3d n = params.anchorNormals->row(idx / 3);
Eigen::Vector3d t1 = perpToVector(n);
Eigen::Vector3d t2 = n.cross(t1);
r[roffset + idx] = normalStiffness * disp.dot(n);
r[roffset + idx + 1] = tangentStiffness * disp.dot(t1);
r[roffset + idx + 2] = tangentStiffness * disp.dot(t2);
int m = rod->curState.centerlineIndices[i] * 3;
if (Jr) {
for (int j = 0; j < 3; j++) {
J.push_back(
Eigen::Triplet<double>(roffset + idx, m + j, normalStiffness * n[j]));
J.push_back(Eigen::Triplet<double>(roffset + idx + 1,
m + j,
tangentStiffness * t1[j]));
J.push_back(Eigen::Triplet<double>(roffset + idx + 2,
m + j,
tangentStiffness * t2[j]));
}
}
idx += 3;
}
dofoffset += 3 * rod->numVertices() + rod->numSegments();
}
roffset += idx;
}
if (config.numAnchors() > 0) {
int idx = 0;
constexpr bool gradientCheck = false;
double gradInc = 1.0e-4;
for (int i = 0; i < config.numAnchors(); i++) {
Anchor a = config.anchors[i];
Rod *rod = config.rods[a.rod];
double posStiffness = a.k_pos;
double dirStiffness = a.k_dir;
int dofoffset = 0;
if (a.rod > 0)
dofoffset += numAccumDofsPerRod[a.rod - 1];
if (a.seg > 0)
dofoffset += 3 * a.seg;
//std::cout << "Anchor " << (i + 1) << std::endl;
//std::cout << "\tdofoffset = " << dofoffset << std::endl;
//std::cout << "\trod = " << a.rod << std::endl;
//std::cout << "\tseg = " << a.seg << std::endl;
double bary = a.bary;
// curvePoint = (1-bary) v_seg + bary v_{seg+1}
Eigen::Vector3d vSeg0 = rod->curState.centerlinePositions.row(a.seg);
Eigen::Vector3d vSeg1 = rod->curState.centerlinePositions.row(a.seg + 1);
Eigen::Vector3d curvePoint = (1 - bary) * vSeg0 + bary * vSeg1;
Eigen::Vector3d t = (vSeg1 - vSeg0).normalized() /*/ (vSeg1 - vSeg0).norm()*/;
Eigen::Vector3d db1 = rod->curState.directors.row(a.seg);
Eigen::Vector3d db2 = t.cross(db1);
double thetaSeg = rod->curState.thetas[a.seg];
Eigen::Vector3d d = db1 * cos(thetaSeg) + db2 * sin(thetaSeg);
// Eigen::Vector3d vStartSeg0 = rod->startState.centerlinePositions.row(a.seg);
// Eigen::Vector3d vStartSeg1 = rod->startState.centerlinePositions.row(a.seg + 1);
// Eigen::Vector3d curvePointStart = (1 - bary) * vStartSeg0 + bary * vStartSeg1;
// Eigen::Vector3d tStart = (vStartSeg1 - vStartSeg0) / (vStartSeg1 - vStartSeg0).norm();
// Eigen::Vector3d db1Start = rod->startState.directors.row(a.seg);
// Eigen::Vector3d db2Start = tStart.cross(db1Start);
// double thetaSegStart = rod->startState.thetas[a.seg];
// Eigen::Vector3d dStart = db1Start * cos(thetaSegStart) + db2Start * sin(thetaSegStart);
//Eigen::Vector3d apos = a.ratio * a.pos + (1 - a.ratio) * curvePointStart;
//Eigen::Vector3d anormal = a.ratio * a.normal + (1 - a.ratio) * dStart;
Eigen::Vector3d apos = a.position;
Eigen::Vector3d anormal = a.normal;
Eigen::Vector3d disp = curvePoint.transpose() - apos.transpose();
Eigen::Vector3d t1 = perpToVector(anormal.transpose());
Eigen::Vector3d t2 = anormal.cross(t1);
//r[roffset + idx + 0] = posStiffness * disp.dot(anormal.transpose());
//r[roffset + idx + 1] = posStiffness * disp.dot(t1);
//r[roffset + idx + 2] = posStiffness * disp.dot(t2);
r[roffset + idx + 0] = posStiffness * disp[0];
r[roffset + idx + 1] = posStiffness * disp[1];
r[roffset + idx + 2] = posStiffness * disp[2];
/*
std::cout << "Anchor Nr. " << i << std::endl;
std::cout << "\tr[" << (roffset + idx + 0) << "] = " << r[roffset + idx + 0] << std::endl;
std::cout << "\tr[" << (roffset + idx + 1) << "] = " << r[roffset + idx + 1] << std::endl;
std::cout << "\tr[" << (roffset + idx + 2) << r[roffset + idx + 2] << std::endl;
*/
int m1 = rod->curState.centerlineIndices[a.seg] * 3;
int m2 = rod->curState.centerlineIndices[a.seg + 1] * 3;
if (Jr) {
for (int j = 0; j < 3; j++) {
//// Derivative w.r.t. v_{seg} of the rod
//J.push_back(Eigen::Triplet<double>(roffset + idx + 0, m1 + j, (1 - bary) * posStiffness * anormal[j]));
//J.push_back(Eigen::Triplet<double>(roffset + idx + 1, m1 + j, (1 - bary) * posStiffness * t1[j]));
//J.push_back(Eigen::Triplet<double>(roffset + idx + 2, m1 + j, (1 - bary) * posStiffness * t2[j]));
//// Derivative w.r.t. v_{seg + 1} of the rod
//J.push_back(Eigen::Triplet<double>(roffset + idx + 0, m2 + j, bary * posStiffness * anormal[j]));
//J.push_back(Eigen::Triplet<double>(roffset + idx + 1, m2 + j, bary * posStiffness * t1[j]));
//J.push_back(Eigen::Triplet<double>(roffset + idx + 2, m2 + j, bary * posStiffness * t2[j]));
J.push_back(Eigen::Triplet<double>(roffset + idx + j,
m1 + j,
(1 - bary) * posStiffness));
J.push_back(
Eigen::Triplet<double>(roffset + idx + j, m2 + j, bary * posStiffness));
}
// Do a little gradient Check
if (gradientCheck) {
double nNumGrad = 0.0;
double t1NumGrad = 0.0;
double t2NumGrad = 0.0;
double nDelta = 0.0;
double t1Delta = 0.0;
double t2Delta = 0.0;
for (int j = 0; j < 3; j++) {
Eigen::Vector3d delta(0.0, 0.0, 0.0);
delta[j] += gradInc;
// Gradients w.r.t. vSeg0
Eigen::Vector3d vSeg0Test = vSeg0 + delta;
curvePoint = (1 - bary) * vSeg0Test + bary * vSeg1;
disp = curvePoint.transpose() - apos.transpose();
nNumGrad = (posStiffness * disp.dot(anormal.transpose())
- r[roffset + idx + 0])
/ gradInc;
t1NumGrad = (posStiffness * disp.dot(t1) - r[roffset + idx + 1]) / gradInc;
t2NumGrad = (posStiffness * disp.dot(t2) - r[roffset + idx + 2]) / gradInc;
nDelta = abs(nNumGrad - (1 - bary) * posStiffness * anormal[j]);
t1Delta = abs(t1NumGrad - (1 - bary) * posStiffness * t1[j]);
t2Delta = abs(t2NumGrad - (1 - bary) * posStiffness * t2[j]);
// Gradients w.r.t. vSeg1
Eigen::Vector3d vSeg1Test = vSeg1 + delta;
curvePoint = (1 - bary) * vSeg0 + bary * vSeg1Test;
disp = curvePoint.transpose() - apos.transpose();
t1 = perpToVector(anormal.transpose());
t2 = anormal.cross(t1);
nNumGrad = (posStiffness * disp.dot(anormal.transpose())
- r[roffset + idx + 0])
/ gradInc;
t1NumGrad = (posStiffness * disp.dot(t1) - r[roffset + idx + 1]) / gradInc;
t2NumGrad = (posStiffness * disp.dot(t2) - r[roffset + idx + 2]) / gradInc;
nDelta = abs(nNumGrad - (1 - bary) * posStiffness * anormal[j]);
t1Delta = abs(t1NumGrad - (1 - bary) * posStiffness * t1[j]);
t2Delta = abs(t2NumGrad - (1 - bary) * posStiffness * t2[j]);
}
}
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
// design variables - Ui anchor positions
for (int j = 0; j < 3; j++) {
//Jui.push_back(Eigen::Triplet<double>(roffset + idx + 0, uianchoroffset + i * 6 + j, posStiffness * -a.normal[j]));
//Jui.push_back(Eigen::Triplet<double>(roffset + idx + 1, uianchoroffset + i * 6 + j, posStiffness * -t1[j]));
//Jui.push_back(Eigen::Triplet<double>(roffset + idx + 2, uianchoroffset + i * 6 + j, posStiffness * -t2[j]));
Jui.push_back(Eigen::Triplet<double>(roffset + idx + j,
uianchoroffset + i * 6 + j,
posStiffness * -1));
}
// design variables - Ui anchor normal
Eigen::Vector3d axis = perpToVectorAxis(a.normal.transpose());
Eigen::Vector3d axn = axis.cross(a.normal);
Eigen::Vector3d daxnnorm = skewSym(axis).transpose() * axn / axn.norm();
Eigen::Matrix3d dt1 = (skewSym(axis) * axn.norm() - axn * daxnnorm.transpose())
/ (axn.norm() * axn.norm());
Eigen::Matrix3d dt2 = skewSym(t1).transpose() + skewSym(a.normal) * dt1;
//for (int j = 0; j < 3; j++)
//{
// Jui.push_back(Eigen::Triplet<double>(roffset + idx + 0, uianchoroffset + i * 6 + 3 + j, posStiffness * disp[j]));
// Jui.push_back(Eigen::Triplet<double>(roffset + idx + 1, uianchoroffset + i * 6 + 3 + j, posStiffness * dt1.col(j).dot(disp)));
// Jui.push_back(Eigen::Triplet<double>(roffset + idx + 2, uianchoroffset + i * 6 + 3 + j, posStiffness * dt2.col(j).dot(disp)));
// Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 0, m1 + j, uianchoroffset + i * 6 + 3 + j));
// Hu_value->push_back((1 - bary) * posStiffness);
// Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 0, m2 + j, uianchoroffset + i * 6 + 3 + j));
// Hu_value->push_back(bary * posStiffness);
// for (int k = 0; k < 3; k++)
// {
// Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 1, m1 + j, uianchoroffset + i * 6 + 3 + k));
// Hu_value->push_back((1 - bary) * posStiffness * dt1(j, k));
// Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 1, m2 + j, uianchoroffset + i * 6 + 3 + k));
// Hu_value->push_back(bary * posStiffness * dt1(j, k));
// Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 2, m1 + j, uianchoroffset + i * 6 + 3 + k));
// Hu_value->push_back((1 - bary) * posStiffness * dt2(j, k));
// Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 2, m2 + j, uianchoroffset + i * 6 + 3 + k));
// Hu_value->push_back(bary * posStiffness * dt2(j, k));
// }
//}
constexpr bool numericalHessian = false;
if (numericalHessian) {
gradInc = 1.0e-6;
Eigen::Vector3d num_daxnnorm;
Eigen::Matrix3d num_dt1;
Eigen::Matrix3d num_dt2;
Eigen::Matrix3d num_r_dn;
Eigen::Matrix3d ana_r_dn;
for (int j = 0; j < 3; j++) {
Eigen::Vector3d _anormal = a.normal;
_anormal[j] += gradInc;
Eigen::Vector3d _axn = axis.cross(_anormal);
Eigen::Vector3d _t1 = _axn / _axn.norm();
Eigen::Vector3d _t2 = _anormal.cross(_t1);
num_daxnnorm[j] = (_axn.norm() - axn.norm()) / gradInc;
num_dt1.col(j) = (_t1 - t1) / gradInc;
num_dt2.col(j) = (_t2 - t2) / gradInc;
num_r_dn(j, 0) = (disp.dot(_anormal.transpose())
- disp.dot(anormal.transpose()))
/ gradInc;
num_r_dn(j, 1) = (disp.dot(_t1) - disp.dot(t1)) / gradInc;
num_r_dn(j, 2) = (disp.dot(_t2) - disp.dot(t2)) / gradInc;
}
ana_r_dn.col(0) = disp;
ana_r_dn.col(1) = dt1.transpose() * disp;
ana_r_dn.col(2) = dt2.transpose() * disp;
int stop = 0;
}
}
#endif
}
// direction constraint for anchor
Eigen::Vector3d axis = d.cross(anormal);
if (axis.norm() == 0.0) {
r[roffset + idx + 3] = 0;
idx += 4;
continue;
}
axis /= axis.norm();
double theta = angle(d, anormal, axis);
double factor = 100 * 0.5 * dirStiffness;
r[roffset + idx + 3] = sqrt(factor) * theta;
if (Jr) {
int m1 = rod->curState.centerlineIndices[a.seg] * 3;
int m2 = rod->curState
.centerlineIndices[((a.seg + 1) % config.rods[a.rod]->numVertices())]
* 3;
int rod1offset = 3 * config.numVertices();
for (int rod = 0; rod < a.rod; rod++) {
rod1offset += config.rods[rod]->numSegments();
}
Eigen::Vector3d dtheta1 = -axis.cross(d);
for (int j = 0; j < 3; j++) {
J.push_back(Eigen::Triplet<double>(roffset + idx + 3,
m1 + j,
sqrt(factor) * dtheta1.dot(t) * d[j]
/ (vSeg1 - vSeg0).norm()));
J.push_back(Eigen::Triplet<double>(roffset + idx + 3,
m2 + j,
-sqrt(factor) * dtheta1.dot(t) * d[j]
/ (vSeg1 - vSeg0).norm()));
}
Eigen::Vector3d d1dtheta = -db1 * sin(thetaSeg) + db2 * cos(thetaSeg);
J.push_back(Eigen::Triplet<double>(roffset + idx + 3,
rod1offset + a.seg,
sqrt(factor) * dtheta1.dot(d1dtheta)));
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
double len = (vSeg1 - vSeg0).norm();
Eigen::Matrix3d dt_de1 = ((1 / len) * Eigen::Matrix<double, 3, 3>::Identity()
- (1 / (len * len * len)) * (vSeg1 - vSeg0)
* (vSeg1 - vSeg0).transpose());
Eigen::Matrix3d dd_de1 = -t * d.transpose() / len;
double axislen = d.cross(anormal).norm();
Eigen::Vector3d daxislen_de1 = -((d.cross(anormal) / axislen).transpose()
* skewSym(anormal) * dd_de1)
.transpose();
Eigen::Matrix3d d2theta1_de1e1 = (dd_de1 * (d.dot(anormal))
+ d * (anormal.transpose() * dd_de1)
- 2 * anormal * (d.transpose() * dd_de1))
/ axislen
- (d.cross(d.cross(anormal))
* daxislen_de1.transpose())
/ (axislen * axislen);
Eigen::Matrix3d dEan_de1e1 = -((d * (t.transpose() * d2theta1_de1e1)
+ d * (dtheta1.transpose() * dt_de1)
+ dtheta1.dot(t) * dd_de1)
* len
- dtheta1.dot(t) * d * t.transpose())
/ (len * len);
Eigen::Vector3d d2theta1_de1theta1 = -(
(d1dtheta.cross(anormal) / axislen
- d.cross(anormal) * axis.dot(d1dtheta.cross(anormal))
/ (axislen * axislen))
.cross(d)
+ axis.cross(d1dtheta));
Eigen::Vector3d dEan_de1theta1 = -d2theta1_de1theta1.dot(t) * d / len
- dtheta1.dot(t) * d1dtheta / len;
double dEan_dtheta1theta1 = d2theta1_de1theta1.dot(d1dtheta)
+ dtheta1.dot(-db1 * cos(thetaSeg)
- db2 * sin(thetaSeg));
for (int h1 = 0; h1 < 3; h1++) {
for (int h2 = 0; h2 < 3; h2++) {
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, m1 + h1, m1 + h2));
H_value->push_back(sqrt(factor) * dEan_de1e1(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, m1 + h1, m2 + h2));
H_value->push_back(-sqrt(factor) * dEan_de1e1(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, m2 + h1, m1 + h2));
H_value->push_back(-sqrt(factor) * dEan_de1e1(h1, h2));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, m2 + h1, m2 + h2));
H_value->push_back(sqrt(factor) * dEan_de1e1(h1, h2));
}
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, m1 + h1, rod1offset + a.seg));
H_value->push_back(-sqrt(factor) * dEan_de1theta1(h1));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, m2 + h1, rod1offset + a.seg));
H_value->push_back(sqrt(factor) * dEan_de1theta1(h1));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, rod1offset + a.seg, m1 + h1));
H_value->push_back(-sqrt(factor) * dEan_de1theta1(h1));
H_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3, rod1offset + a.seg, m2 + h1));
H_value->push_back(sqrt(factor) * dEan_de1theta1(h1));
}
H_index->push_back(Eigen::Triplet<int>(roffset + idx + 3,
rod1offset + a.seg,
rod1offset + a.seg));
H_value->push_back(sqrt(factor) * dEan_dtheta1theta1);
// design variables - Ui anchor normal
Eigen::Vector3d dtheta1_dan = axis.cross(a.normal);
Eigen::Vector3d daxislen_dan
= ((d.cross(anormal) / axislen).transpose() * skewSym(d)).transpose();
Eigen::Vector3d daxislen_dan2 = skewSym(d).transpose()
* (d.cross(anormal) / axislen);
Eigen::Matrix3d d2theta1_de1an
= (d * d.transpose() - Eigen::Matrix<double, 3, 3>::Identity()) / axislen
- (d.cross(d.cross(anormal)) * daxislen_dan.transpose())
/ (axislen * axislen);
Eigen::Matrix3d d2theta1_danan = (anormal * d.transpose()
+ Eigen::Matrix<double, 3, 3>::Identity()
* anormal.dot(d)
- 2 * d * anormal.transpose())
/ axislen
+ (anormal.cross(d.cross(anormal))
* daxislen_dan.transpose())
/ (axislen * axislen);
Eigen::Matrix3d dEan_de1an = -d * (t.transpose() * d2theta1_de1an)
/ (vSeg1 - vSeg0).norm();
Eigen::Matrix3d dEan_danan = d2theta1_danan / anormal.norm()
- (dtheta1_dan
* (anormal / anormal.norm()).transpose())
/ (anormal.norm() * anormal.norm());
Eigen::Vector3d dEan_dthetaan = (d1dtheta.transpose() * d2theta1_de1an)
.transpose();
Eigen::Matrix3d d2theta1_dane1 = -(
skewSym(anormal).transpose() * dd_de1 / axislen
- (d.cross(anormal) * daxislen_de1.transpose()) / (axislen * axislen)
+ skewSym(axis)
* dd_de1); // no idea why the minus needs to be there, but the numerical version has it
//dEan_de1an = 0.5 * (dEan_de1an + d2theta1_dane1) / anormal.norm();
constexpr bool numericalHessian = false;
gradInc = 1.0e-6;
Eigen::Vector3d numGrad;
Eigen::Vector3d num_daxislen_dan;
Eigen::Matrix3d num_d2theta1_danan;
Eigen::Matrix3d num_dEan_de1e1;
Eigen::Matrix3d num_dEan_de1an;
Eigen::Matrix3d num_dEan_danan;
Eigen::Matrix3d num_d2theta1_danan2;
Eigen::Vector3d num_dtheta1_de1;
Eigen::Vector3d num_dtheta1_dan;
Eigen::Matrix3d num_d2Ean_danan2;
Eigen::Matrix3d num_d2theta1_danan3;
Eigen::Matrix3d num_d2theta1_de1an;
Eigen::Matrix3d num_d2Ean_de1an;
double num_dtheta_dthetaSeg;
Eigen::Vector3d num_d1_dtheta;
Eigen::Vector3d num_d2Ean_dthetaan;
Eigen::Vector3d num_daxislen_de1;
Eigen::Matrix3d num_d2theta1_de1e1;
Eigen::Vector3d num_dEan_de1theta1;
double num_dEan_dtheta1theta1;
Eigen::Vector3d num_d2theta1_de1theta1;
Eigen::Matrix3d num_d2theta1_dane1;
for (int h1 = 0; h1 < 3; h1++) {
Jui.push_back(Eigen::Triplet<double>(roffset + idx + 3,
uianchoroffset + i * 6 + 3 + h1,
sqrt(factor) * dtheta1_dan[h1]
/ anormal.norm()));
for (int h2 = 0; h2 < 3; h2++) {
if (numericalHessian) {
Eigen::Vector3d vSeg1_h1h2 = vSeg1;
Eigen::Vector3d vSeg1_h1 = vSeg1;
Eigen::Vector3d vSeg1_h2 = vSeg1;
vSeg1_h1h2[h1] += gradInc;
vSeg1_h1h2[h2] += gradInc;
vSeg1_h1[h1] += gradInc;
vSeg1_h2[h2] += gradInc;
Eigen::Vector3d t_h1h2 = (vSeg1_h1h2 - vSeg0)
/ (vSeg1_h1h2 - vSeg0).norm();
Eigen::Vector3d t_h1 = (vSeg1_h1 - vSeg0)
/ (vSeg1_h1 - vSeg0).norm();
Eigen::Vector3d t_h2 = (vSeg1_h2 - vSeg0)
/ (vSeg1_h2 - vSeg0).norm();
Eigen::Vector3d d_h1h2 = parallelTransport(d, t, t_h1h2);
Eigen::Vector3d d_h1 = parallelTransport(d, t, t_h1);
Eigen::Vector3d d_h2 = parallelTransport(d, t, t_h2);
double thetaSeg_h2 = thetaSeg + gradInc;
Eigen::Vector3d d_theta = db1 * cos(thetaSeg_h2)
+ db2 * sin(thetaSeg_h2);
Eigen::Vector3d anormal_h1h2 = anormal;
Eigen::Vector3d anormal_h1 = anormal;
Eigen::Vector3d anormal_h2 = anormal;
anormal_h1h2[h1] += gradInc;
anormal_h1h2[h2] += gradInc;
anormal_h1[h1] += gradInc;
anormal_h2[h2] += gradInc;
//anormal_h1h2 = anormal_h1h2 / anormal_h1h2.norm();
//anormal_h1 = anormal_h1 / anormal_h1.norm();
//anormal_h2 = anormal_h2 / anormal_h2.norm();
Eigen::Vector3d axis_eh1eh2 = d_h1h2.cross(anormal);
Eigen::Vector3d axis_eh1 = d_h1.cross(anormal);
Eigen::Vector3d axis_eh2 = d_h2.cross(anormal);
Eigen::Vector3d axis_ah1ah2 = d.cross(anormal_h1h2);
Eigen::Vector3d axis_ah1 = d.cross(anormal_h1);
Eigen::Vector3d axis_ah2 = d.cross(anormal_h2);
Eigen::Vector3d axis_eh1ah2 = d_h1.cross(anormal_h2);
Eigen::Vector3d axis_theta = d_theta.cross(anormal);
Eigen::Vector3d axis_thetaah2 = d_theta.cross(anormal_h2);
axis_eh1eh2 /= axis_eh1eh2.norm();
axis_eh1 /= axis_eh1.norm();
axis_eh2 /= axis_eh2.norm();
axis_ah1ah2 /= axis_ah1ah2.norm();
axis_ah1 /= axis_ah1.norm();
axis_ah2 /= axis_ah2.norm();
axis_theta /= axis_theta.norm();
axis_thetaah2 /= axis_thetaah2.norm();
axis_eh1ah2 /= axis_eh1ah2.norm();
double theta_eh1eh2 = angle(d_h1h2, anormal, axis_eh1eh2);
double theta_eh1 = angle(d_h1, anormal, axis_eh1);
double theta_eh2 = angle(d_h2, anormal, axis_eh2);
double theta_ah1ah2 = angle(d, anormal_h1h2, axis_ah1ah2);
double theta_ah1 = angle(d, anormal_h1, axis_ah1);
double theta_ah2 = angle(d, anormal_h2, axis_ah2);
double theta_eh1ah2 = angle(d_h1, anormal_h2, axis_eh1ah2);
double theta_theta = angle(d_theta, anormal, axis_theta);
double theta_thetaah2 = angle(d_theta, anormal_h2, axis_thetaah2);
double factor = 100 * 0.5 * dirStiffness;
double r_eh1eh2 = theta_eh1eh2;
double r_eh1 = sqrt(factor) * theta_eh1;
double r_eh2 = sqrt(factor) * theta_eh2;
double r_ah1ah2 = sqrt(factor) * theta_ah1ah2;
double r_ah1 = sqrt(factor) * theta_ah1;
double r_ah2 = sqrt(factor) * theta_ah2;
double r_eh1ah2 = sqrt(factor) * theta_eh1ah2;
num_daxislen_dan[h1] = (d.cross(anormal_h1).norm() - axislen)
/ gradInc;
//num_dEan_de1e1(h1, h2) = (r_eh1eh2 + r[roffset + idx + 3] - r_eh1 - r_eh2) / (gradInc*gradInc);
//num_dEan_danan(h1, h2) = (r_ah1ah2 + r[roffset + idx + 3] - r_ah1 - r_ah2) / (gradInc*gradInc);
//num_dEan_de1an(h1, h2) = (r_eh1ah2 + r[roffset + idx + 3] - r_eh1 - r_ah2) / (gradInc*gradInc);
//num_d2theta1_danan(h1, h2) = (theta_ah1ah2 + theta - theta_ah1 - theta_ah2) / (gradInc*gradInc);
num_dtheta1_de1[h2] = (angle(d_h2, anormal, axis_eh2)
- angle(d, anormal, axis))
/ gradInc;
num_dtheta1_dan[h2] = (angle(d, anormal_h2, axis_ah2)
- angle(d, anormal, axis))
/ gradInc;
num_d2theta1_danan2.col(h2) = ((axis.cross(anormal_h2)
- axis.cross(anormal))
+ (axis_ah2.cross(anormal)
- axis.cross(anormal)))
/ gradInc;
num_d2theta1_danan3.col(h2) = (axis_ah2.cross(anormal_h2)
- axis.cross(anormal))
/ gradInc;
num_d2Ean_danan2.col(h2) = (axis_ah2.cross(anormal_h2)
/ anormal_h2.norm()
- axis.cross(anormal) / anormal.norm())
/ gradInc;
num_d2theta1_de1an.col(h2) = (-axis_ah2.cross(d) + axis.cross(d))
/ gradInc;
num_d2Ean_de1an.col(h2) = (axis_ah2.cross(d).dot(t) * d
/ (vSeg1 - vSeg0).norm()
+ dtheta1.dot(t) * d
/ (vSeg1 - vSeg0).norm())
/ gradInc;
num_dtheta_dthetaSeg = (theta_theta - theta) / gradInc;
num_d2Ean_dthetaan[h2] = (-axis_ah2.cross(d).dot(
-db1 * sin(thetaSeg)
+ db2 * cos(thetaSeg))
+ axis.cross(d).dot(-db1 * sin(thetaSeg)
+ db2 * cos(thetaSeg)))
/ gradInc;
num_d1_dtheta = (d_theta - d) / gradInc;
num_daxislen_de1[h2] = (d_h2.cross(anormal).norm() - axislen)
/ gradInc;
num_d2theta1_de1e1.col(h2) = (-axis_eh2.cross(d_h2) - dtheta1)
/ gradInc;
num_dEan_de1e1.col(h2) = (axis_eh2.cross(d_h2).dot(t_h2) * d_h2
/ (vSeg1_h2 - vSeg0).norm()
+ dtheta1.dot(t) * d
/ (vSeg1 - vSeg0).norm())
/ gradInc;
num_dEan_de1theta1 = (axis_theta.cross(d_theta).dot(t) * d_theta
/ (vSeg1 - vSeg0).norm()
+ dtheta1.dot(t) * d / (vSeg1 - vSeg0).norm())
/ gradInc;
num_dEan_dtheta1theta1 = (-axis_theta.cross(d_theta).dot(
-db1 * sin(thetaSeg_h2)
+ db2 * cos(thetaSeg_h2))
- dtheta1.dot(d1dtheta))
/ gradInc;
num_d2theta1_de1theta1 = (-axis_theta.cross(d_theta) + axis.cross(d))
/ gradInc;
num_d2theta1_dane1.col(h2) = (axis_eh2.cross(a.normal)
- axis.cross(a.normal))
/ gradInc;
num_dEan_de1an(h1, h2) = (theta_eh1ah2 + theta - theta_eh1
- theta_ah1)
/ (2 * gradInc);
}
Hu_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3,
m1 + h1,
uianchoroffset + i * 6 + 3 + h2));
Hu_value->push_back(-sqrt(factor) * dEan_de1an(h1, h2));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + idx + 3,
m2 + h1,
uianchoroffset + i * 6 + 3 + h2));
Hu_value->push_back(sqrt(factor) * dEan_de1an(h1, h2));
//Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 3, uianchoroffset + i * 6 + 3 + h1, uianchoroffset + i * 6 + 3 + h2));
//Hu_value->push_back(sqrt(factor) * dEan_danan(h1, h2));
}
Hu_index->push_back(Eigen::Triplet<int>(roffset + idx + 3,
rod1offset + a.seg,
uianchoroffset + i * 6 + 3 + h1));
//Hu_value->push_back(-sqrt(factor)*d1dtheta[h1]);
Hu_value->push_back(sqrt(factor) * dEan_dthetaan[h1]);
//Hu_value->push_back(sqrt(factor)*num_d2Ean_dthetaan[h1]);
}
int stop = 0;
}
#endif
}
idx += 4;
}
roffset += idx;
}
#endif
// compute energy terms for rod intersections
int incCount = 0;
for (int i = 0; i < config.numIntersections(); i++) { //iterate over all intersections
Intersection intersection = config.intersections[i];
for (int rodSegmentIndex_intersection = 0;
rodSegmentIndex_intersection < intersection.rodSegmentIndices.rows();
rodSegmentIndex_intersection++) //iterate over incident segments of the intersection
{
const int ri_intersection = intersection.rodSegmentIndices(rodSegmentIndex_intersection,
0);
const int si_intersection = intersection.rodSegmentIndices(rodSegmentIndex_intersection,
1);
const Rod &rod = *config.rods[ri_intersection];
const int numVertices_rod = rod.numVertices();
const int nsegsoffset = (ri_intersection > 0 ? numAccumDofsPerRod[ri_intersection - 1]
: 0);
thetaoffset = config.numVertices() * 3 + nsegsoffset;
int j = si_intersection;
// int m0 = curstate.centerlineIndices[(j - 1) % numVertices_rod];
// int m1 = curstate.centerlineIndices[j];
// int m2 = curstate.centerlineIndices[(j + 1) % numVertices_rod];
int numInc = 0; //??
Eigen::MatrixXi incMat; //??
if ((si_intersection > 0
&& si_intersection < rod.numVertices() - 1) //not first and not last segment
|| rod.isClosed()) // in- and outgoing
{
incMat.resize(2, 3);
incMat << (j - 1) % numVertices_rod, j, j - 1, j, (j + 1) % numVertices_rod,
j; //??
numInc = 2;
} else if (si_intersection > 0) // last segment and not closed
{
incMat.resize(1, 3);
incMat << (j - 1) % numVertices_rod, j, j - 1;
numInc = 1;
} else // only outgoing //first segment and not closed
{
incMat.resize(1, 3);
incMat << j, (j + 1) % numVertices_rod, j;
numInc = 1;
}
//What is he doing here?
const RodState &curstate = config.rods[ri_intersection]->curState;
const RodState &startstate = config.rods[ri_intersection]->startState;
int m0, m1;
for (int inc = 0; inc < numInc; inc++) {
m0 = curstate.centerlineIndices[incMat(inc, 0)];
m1 = curstate.centerlineIndices[incMat(inc, 1)];
j = incMat(inc, 2);
Eigen::Vector3d cv0 = curstate.centerlinePositions.row(incMat(inc, 0)).transpose();
Eigen::Vector3d cv1 = curstate.centerlinePositions.row(incMat(inc, 1)).transpose();
Eigen::Vector3d sv0 = startstate.centerlinePositions.row(incMat(inc, 0)).transpose();
Eigen::Vector3d sv1 = startstate.centerlinePositions.row(incMat(inc, 1)).transpose();
Eigen::Vector3d ct01 = (cv1 - cv0).normalized();
Eigen::Vector3d st01 = intersection.rotationMatrix * ((sv1 - sv0).normalized());
Eigen::Vector3d kb = 2.0 * ct01.cross(st01) / (1.0 + ct01.dot(st01));
Eigen::Vector3d cdb1 = curstate.directors.row(j).transpose();
Eigen::Vector3d cdb2 = ct01.cross(cdb1);
Eigen::Vector3d sdb1 = intersection.rotationMatrix
* startstate.directors.row(j).transpose();
Eigen::Vector3d sdb2 = st01.cross(sdb1);
double theta = curstate.thetas[j];
Eigen::Vector3d cd1 = cdb1 * cos(theta) + cdb2 * sin(theta);
Eigen::Vector3d cd2 = -cdb1 * sin(theta) + cdb2 * cos(theta);
Eigen::Vector3d sd1 = sdb1;
Eigen::Vector3d sd2 = sdb2;
//bending
double k1 = 0.5 * ((cd2 + sd2).dot(kb));
double k2 = 0.5 * ((cd1 + sd1).dot(kb));
double len = 0.5 * rod.vSegParams(j, 3);
double width = rod.widths[j];
// double factor1 = 0.5 * rod.vSegParams(j, 1) * rod.params.thickness
// * rod.params.thickness * rod.params.thickness / len;
// double factor2 = 0.5 * rod.vSegParams(j, 1) * rod.params.thickness / len;
//assuming rectangle cross section
const double debug_momentOfInertia_first = width
* std::pow(rod.params.thickness, 3.0)
/ 12;
const double debug_momentOfInertia_second = rod.params.thickness
* std::pow(width, 3.0) / 12;
const double debug_bendingStiffness_11 = debug_momentOfInertia_first
* rod.vSegParams(j, 1);
const double debug_bendingStiffness_22 = debug_momentOfInertia_second
* rod.vSegParams(j, 1);
double factor1 = debug_bendingStiffness_11 / len;
double factor2 = debug_bendingStiffness_22 / len;
double vertr1 = sqrt(factor1) * (k1 - 0);
double vertr2 = sqrt(factor2) * (k2 - 0);
//twisting
Eigen::Vector3d sd1t = parallelTransport(sd1, st01, ct01); //??
double psi = angle(sd1t, cd1, ct01);
const double G = rod.vSegParams(j, 2);
const double torsionalConstant_J = 0.141 * std::pow(rod.params.thickness, 4);
const double factor3 = /*0.5 **/ torsionalConstant_J * G / len;
// double factor3 = 0.5 * rod.vSegParams(j, 2) * rod.params.thickness
// * rod.params.thickness * rod.params.thickness / len; //
double vertr3 = sqrt(factor3) * psi;
r[roffset + incCount] = vertr1;
r[roffset + incCount + 1] = vertr2;
r[roffset + incCount + 2] = vertr3;
if (Jr) {
Eigen::Vector3d ttilde = (ct01 + st01) / (1.0 + ct01.dot(st01));
Eigen::Vector3d d1tilde = (cd1 + sd1) / (1.0 + ct01.dot(st01));
Eigen::Vector3d d2tilde = (cd2 + sd2) / (1.0 + ct01.dot(st01));
// Gradient for euler rotation matrices
Eigen::Matrix3d rot0 = eulerRotation(0, 0, 0, 0);
Eigen::Matrix3d rot1 = eulerRotation(0, 0, 0, 1);
Eigen::Matrix3d rot2 = eulerRotation(0, 0, 0, 2);
// Gradient for dkb/de (euler angles) = R' * st12
Eigen::Vector3d dkb0 = 2
* (ct01.cross(rot0 * st01) * (1 + ct01.dot(st01))
- ct01.cross(st01) * (ct01.dot(rot0 * st01)))
/ ((1 + ct01.dot(st01)) * (1 + ct01.dot(st01)));
Eigen::Vector3d dkb1 = 2
* (ct01.cross(rot1 * st01) * (1 + ct01.dot(st01))
- ct01.cross(st01) * (ct01.dot(rot1 * st01)))
/ ((1 + ct01.dot(st01)) * (1 + ct01.dot(st01)));
Eigen::Vector3d dkb2 = 2
* (ct01.cross(rot2 * st01) * (1 + ct01.dot(st01))
- ct01.cross(st01) * (ct01.dot(rot2 * st01)))
/ ((1 + ct01.dot(st01)) * (1 + ct01.dot(st01)));
double dk1e0 = 0.5 * ((rot0 * sd2).dot(kb) + (cd2 + sd2).dot(dkb0));
double dk1e1 = 0.5 * ((rot1 * sd2).dot(kb) + (cd2 + sd2).dot(dkb1));
double dk1e2 = 0.5 * ((rot2 * sd2).dot(kb) + (cd2 + sd2).dot(dkb2));
double dk2e0 = 0.5 * ((rot0 * sd1).dot(kb) + (cd1 + sd1).dot(dkb0));
double dk2e1 = 0.5 * ((rot1 * sd1).dot(kb) + (cd1 + sd1).dot(dkb1));
double dk2e2 = 0.5 * ((rot2 * sd1).dot(kb) + (cd1 + sd1).dot(dkb2));
// Bending Gradients for dk1/dx_(i-1) and dk1/dx_i
Eigen::Vector3d dk11 = 1.0 / (cv1 - cv0).norm()
* (-k1 * ttilde + st01.cross(d2tilde));
for (int k = 0; k < 3; k++) {
// Gradients for dEb1/dx_(i-1), dEb1/dx_i and dEb1/dx_(i+1)
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m0)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + incCount,
m0 * 3 + k,
-sqrt(factor1) * dk11[k]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + incCount,
m1 * 3 + k,
sqrt(factor1) * dk11[k]));
#ifdef EXPLICIT_BC
}
#endif
}
J.push_back(Eigen::Triplet<double>(roffset + incCount,
euleroffset + i * 3 + 0,
-sqrt(factor1) * dk1e0));
J.push_back(Eigen::Triplet<double>(roffset + incCount,
euleroffset + i * 3 + 1,
-sqrt(factor1) * dk1e1));
J.push_back(Eigen::Triplet<double>(roffset + incCount,
euleroffset + i * 3 + 2,
-sqrt(factor1) * dk1e2));
// Bending Gradients for dk2/dx_(i-1) and dk2/dx_i
Eigen::Vector3d dk21 = 1.0 / (cv1 - cv0).norm()
* (-k2 * ttilde + st01.cross(d1tilde));
for (int k = 0; k < 3; k++) {
// Gradients for dEb2/dx_(i-1), dEb2/dx_i and dEb2/dx_(i+1)
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m0)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + incCount + 1,
m0 * 3 + k,
-sqrt(factor2) * dk21[k]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + incCount + 1,
m1 * 3 + k,
sqrt(factor2) * dk21[k]));
#ifdef EXPLICIT_BC
}
#endif
}
J.push_back(Eigen::Triplet<double>(roffset + incCount + 1,
euleroffset + i * 3 + 0,
-sqrt(factor2) * dk2e0));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 1,
euleroffset + i * 3 + 1,
-sqrt(factor2) * dk2e1));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 1,
euleroffset + i * 3 + 2,
-sqrt(factor2) * dk2e2));
// Material curvature gradients dd/dtheta1 and dd/theta2
Eigen::Vector3d Dcd1 = -cdb1 * sin(theta) + cdb2 * cos(theta);
Eigen::Vector3d Dcd2 = -cdb1 * cos(theta) - cdb2 * sin(theta);
// Bending Gradients for dEb1/dtheta1, dEb2/dtheta1, dEb1/dtheta2 and dEb2/dtheta2
J.push_back(Eigen::Triplet<double>(roffset + incCount,
thetaoffset + j,
0.5 * sqrt(factor1) * Dcd2.dot(kb)));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 1,
thetaoffset + j,
0.5 * sqrt(factor2) * Dcd1.dot(kb)));
//twisting
// Twisting Gradient dtheta1/dx and dtheta2/dx
Eigen::Vector3d dpsi_dx = 0.5 * kb / (cv1 - cv0).norm();
for (int k = 0; k < 3; k++) {
// Gradients dEt/dx_(i-1), dEt/dx_i and dEt/dx_(i+1)
#ifdef EXPLICIT_BC
if (!pJob->constrainedVertices.contains(m0)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2,
m0 * 3 + k,
-sqrt(factor3) * dpsi_dx[k]));
#ifdef EXPLICIT_BC
}
if (!pJob->constrainedVertices.contains(m1)) {
#endif
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2,
m1 * 3 + k,
sqrt(factor3) * dpsi_dx[k]));
#ifdef EXPLICIT_BC
}
#endif
}
// Gradients dEt/dtheta1 and dEt/dtheta2
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2,
thetaoffset + j,
sqrt(factor3)));
//Gradient dpsi/de
Eigen::Vector3d dsd1t0 = parallelDerivative(sd1, st01, ct01, rot0);
Eigen::Vector3d dsd1t1 = parallelDerivative(sd1, st01, ct01, rot1);
Eigen::Vector3d dsd1t2 = parallelDerivative(sd1, st01, ct01, rot2);
double dpsi0 = ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t0);
double dpsi1 = ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t1);
double dpsi2 = ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t2);
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2,
euleroffset + i * 3 + 0,
-sqrt(factor3) * dpsi0));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2,
euleroffset + i * 3 + 1,
-sqrt(factor3) * dpsi1));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2,
euleroffset + i * 3 + 2,
-sqrt(factor3) * dpsi2));
// gradient check
if (false) {
Eigen::Vector3d dk1vec(dk1e0, dk1e1, dk1e2);
Eigen::Vector3d dk2vec(dk2e0, dk2e1, dk2e2);
Eigen::Vector3d dpsivec(dpsi0, dpsi1, dpsi2);
Eigen::Vector3d num_dk1vec;
Eigen::Vector3d num_dk2vec;
Eigen::Vector3d num_dpsivec;
double gradInc = 1.0e-6;
for (int dim = 0; dim < 3; dim++) {
Eigen::Vector3d delta(0, 0, 0);
delta[dim] += gradInc;
Eigen::Matrix3d rotInc = eulerRotation(delta[0], delta[1], delta[2], -1);
Eigen::Vector3d _cv0 = curstate.centerlinePositions.row(incMat(inc, 0))
.transpose();
Eigen::Vector3d _cv1 = curstate.centerlinePositions.row(incMat(inc, 1))
.transpose();
Eigen::Vector3d _sv0
= startstate.centerlinePositions.row(incMat(inc, 0)).transpose();
Eigen::Vector3d _sv1
= startstate.centerlinePositions.row(incMat(inc, 1)).transpose();
Eigen::Vector3d _ct01 = (_cv1 - _cv0) / (_cv1 - _cv0).norm();
Eigen::Vector3d _st01 = rotInc * intersection.rotationMatrix
* ((_sv1 - _sv0) / (_sv1 - _sv0).norm());
Eigen::Vector3d _kb = 2.0 * _ct01.cross(_st01)
/ (1.0 + _ct01.dot(_st01));
Eigen::Vector3d _cdb1 = curstate.directors.row(j).transpose();
Eigen::Vector3d _cdb2 = _ct01.cross(_cdb1);
Eigen::Vector3d _sdb1 = rotInc * intersection.rotationMatrix
* startstate.directors.row(j).transpose();
Eigen::Vector3d _sdb2 = _st01.cross(_sdb1);
double _theta = curstate.thetas[j];
Eigen::Vector3d _cd1 = _cdb1 * cos(_theta) + _cdb2 * sin(_theta);
Eigen::Vector3d _cd2 = -_cdb1 * sin(_theta) + _cdb2 * cos(_theta);
Eigen::Vector3d _sd1 = _sdb1;
Eigen::Vector3d _sd2 = _sdb2;
double _k1 = 0.5 * ((_cd2 + _sd2).dot(_kb));
double _k2 = 0.5 * ((_cd1 + _sd1).dot(_kb));
double _len = 0.5 * rod.vSegParams(j, 3);
double _width = rod.widths[j];
double _factor1 = 0.5 * rod.vSegParams(j, 1) * rod.params.thickness
* rod.params.thickness * rod.params.thickness / _len;
double _factor2 = 0.5 * rod.vSegParams(j, 1) * rod.params.thickness
/ _len;
double _vertr1 = sqrt(_factor1 * _width) * (_k1 - 0);
double _vertr2 = sqrt(_factor2 * _width * _width * _width) * (_k2 - 0);
Eigen::Vector3d _sd1t = parallelTransport(_sd1, _st01, _ct01);
double _psi = angle(_sd1t, _cd1, _ct01);
double _factor3 = 0.5 * rod.vSegParams(j, 2) * rod.params.thickness
* rod.params.thickness * rod.params.thickness / _len;
double _vertr3 = sqrt(_factor3 * _width) * _psi;
double _r1 = _vertr1;
double _r2 = _vertr2;
double _r3 = _vertr3;
double k1numgrad = (_k1 - k1) / gradInc;
double k2numgrad = (_k2 - k2) / gradInc;
double k1diff = k1numgrad - dk1vec[dim];
double k2diff = k2numgrad - dk2vec[dim];
num_dk1vec[dim] = k1numgrad;
num_dk2vec[dim] = k2numgrad;
num_dpsivec[dim] = (_psi - psi) / gradInc;
}
}
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
double chi = 1 + ct01.dot(st01);
double len01 = (cv1 - cv0).norm();
double d2k1_dtheta1theta1 = -kb.dot(0.5 * cd2);
double d2k2_dtheta1theta1 = -kb.dot(0.5 * cd1);
Eigen::Vector3d d2k1_de01theta1 = 1 / chi * (-st01 / len01).cross(cd1)
+ (ttilde / len01) * kb.dot(0.5 * cd1);
Eigen::Vector3d d2k2_de01theta1 = -1 / chi * (-st01 / len01).cross(cd2)
- (ttilde / len01) * kb.dot(0.5 * cd2);
//Eigen::Matrix3d d2k1_de01e01 = 1 / (len01*len01) * (2 * k1 * ttilde * ttilde.transpose() - st01.cross(d2tilde) * ttilde.transpose() - ttilde * st01.cross(d2tilde).transpose() + 0.5 * (kb * cd2.transpose() + cd2 * kb.transpose()) - (k1 / chi + 0.5 * cd2.dot(kb)) * (Eigen::Matrix<double, 3, 3>::Identity() - ct01 * ct01.transpose()) + 0.25 * (kb.cross(ct01) * cd1.transpose() + cd1 * kb.cross(ct01).transpose()));
//Eigen::Matrix3d d2k2_de01e01 = 1 / (len01*len01) * (2 * k2 * ttilde * ttilde.transpose() + st01.cross(d1tilde) * ttilde.transpose() + ttilde * st01.cross(d1tilde).transpose() - 0.5 * (kb * cd1.transpose() + cd1 * kb.transpose()) - (k2 / chi - 0.5 * cd1.dot(kb)) * (Eigen::Matrix<double, 3, 3>::Identity() - ct01 * ct01.transpose()) + 0.25 * (kb.cross(ct01) * cd2.transpose() + cd2 * kb.cross(ct01).transpose()));
//Eigen::Matrix3d d2psi_d01d01 = 1 / (4 * len01*len01) * (kb * (ct01 + (ct01 + st01) / chi).transpose() + (ct01 + (ct01 + st01) / chi) * kb.transpose());
Eigen::Matrix3d d2k1_de01e01
= 1 / (len01 * len01)
* (2 * k1 * ttilde * ttilde.transpose()
- st01.cross(d2tilde) * ttilde.transpose()
- ttilde * st01.cross(d2tilde).transpose()
+ 0.5 * kb * cd2.transpose()
- k1 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
- ct01 * ct01.transpose()));
Eigen::Matrix3d d2k2_de01e01
= 1 / (len01 * len01)
* (2 * k2 * ttilde * ttilde.transpose()
- st01.cross(d1tilde) * ttilde.transpose()
- ttilde * st01.cross(d1tilde).transpose()
+ 0.5 * kb * cd1.transpose()
- k2 / chi
* (Eigen::Matrix<double, 3, 3>::Identity()
- ct01 * ct01.transpose()));
Eigen::Matrix3d d2psi_d01d01
= 1 / (2 * len01 * len01)
* (-2 * skewSym(st01) / chi
- kb * ((ct01 + st01) / chi + ct01).transpose());
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
thetaoffset + j,
thetaoffset + j));
H_value->push_back(sqrt(factor1) * d2k1_dtheta1theta1);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
thetaoffset + j,
thetaoffset + j));
H_value->push_back(sqrt(factor2) * d2k2_dtheta1theta1);
double df1_dkb = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.widths[j])
/ (4 * len * sqrt(factor1 * rod.widths[j]));
double df1_dw = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.vSegParams(j, 1))
/ (4 * len * sqrt(factor1 * rod.widths[j]));
double df1_dt = (3 * rod.params.thickness * rod.params.thickness
* rod.vSegParams(j, 1) * rod.widths[j])
/ (4 * len * sqrt(factor1 * rod.widths[j]));
double df2_dkb = (rod.params.thickness * rod.widths[j] * rod.widths[j]
* rod.widths[j])
/ (4 * len
* sqrt(factor2 * rod.widths[j] * rod.widths[j]
* rod.widths[j]));
double df2_dw = (3 * rod.params.thickness * rod.vSegParams(j, 1)
* rod.widths[j] * rod.widths[j])
/ (4 * len
* sqrt(factor2 * rod.widths[j] * rod.widths[j]
* rod.widths[j]));
double df2_dt = (rod.vSegParams(j, 1) * rod.widths[j] * rod.widths[j]
* rod.widths[j])
/ (4 * len
* sqrt(factor2 * rod.widths[j] * rod.widths[j]
* rod.widths[j]));
double df3_dkt = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.widths[j])
/ (4 * len * sqrt(factor3));
double df3_dw = (rod.params.thickness * rod.params.thickness
* rod.params.thickness * rod.vSegParams(j, 2))
/ (4 * len * sqrt(factor3));
double df3_dt = (3 * rod.params.thickness * rod.params.thickness
* rod.vSegParams(j, 2) * rod.widths[j])
/ (4 * len * sqrt(factor3));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j,
df1_dkb * (k1 - 0)));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j,
df1_dw * (k1 - 0)));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j,
df1_dt * (k1 - 0)));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount + 1,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j,
df2_dkb * (k2 - 0)));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount + 1,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j,
df2_dw * (k2 - 0)));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount + 1,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j,
df2_dt * (k2 - 0)));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount + 2,
uistiffoffset + 2 * nsegsTotal + nsegsoffset + j,
df3_dkt * psi));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount + 2,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j,
df3_dw * psi));
Jui.push_back(
Eigen::Triplet<double>(roffset + incCount + 2,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j,
df3_dt * psi));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 0,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dkb * dk1e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 1,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dkb * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 2,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dkb * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 0,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dkb * dk2e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 1,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dkb * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 2,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dkb * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 0,
uistiffoffset + 2 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dkt * dk2e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 1,
uistiffoffset + 2 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dkt * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 2,
uistiffoffset + 2 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dkt * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
thetaoffset + j,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(0.5 * df1_dkb * Dcd2.dot(kb));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
thetaoffset + j,
uistiffoffset + 1 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(0.5 * df2_dkb * Dcd1.dot(kb));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
thetaoffset + j,
uistiffoffset + 2 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(df3_dkt);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 0,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dw * dk1e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 1,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dw * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 2,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dw * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 0,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dw * dk2e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 1,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dw * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 2,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dw * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 0,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dw * dk2e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 1,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dw * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 2,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dw * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
thetaoffset + j,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(0.5 * df1_dw * Dcd2.dot(kb));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
thetaoffset + j,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(0.5 * df2_dw * Dcd1.dot(kb));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
thetaoffset + j,
uistiffoffset + 3 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(df3_dw);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 0,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dt * dk1e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 1,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dt * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
euleroffset + i * 3 + 2,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df1_dt * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 0,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dt * dk2e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 1,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dt * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
euleroffset + i * 3 + 2,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df2_dt * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 0,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dt * dk2e0);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 1,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dt * dk1e1);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
euleroffset + i * 3 + 2,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(-df3_dt * dk1e2);
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount,
thetaoffset + j,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(0.5 * df1_dt * Dcd2.dot(kb));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 1,
thetaoffset + j,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(0.5 * df2_dt * Dcd1.dot(kb));
Hu_index->push_back(
Eigen::Triplet<int>(roffset + incCount + 2,
thetaoffset + j,
uistiffoffset + 4 * nsegsTotal + nsegsoffset + j));
Hu_value->push_back(df3_dt);
for (int h1 = 0; h1 < 3; h1++) {
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m0 * 3 + h1,
thetaoffset + j));
H_value->push_back(-sqrt(factor1) * d2k1_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
thetaoffset + j,
m0 * 3 + h1));
H_value->push_back(-sqrt(factor1) * d2k1_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m1 * 3 + h1,
thetaoffset + j));
H_value->push_back(sqrt(factor1) * d2k1_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
thetaoffset + j,
m1 * 3 + h1));
H_value->push_back(sqrt(factor1) * d2k1_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m0 * 3 + h1,
thetaoffset + j));
H_value->push_back(-sqrt(factor2) * d2k2_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
thetaoffset + j,
m0 * 3 + h1));
H_value->push_back(-sqrt(factor2) * d2k2_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m1 * 3 + h1,
thetaoffset + j));
H_value->push_back(sqrt(factor2) * d2k2_de01theta1[h1]);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
thetaoffset + j,
m1 * 3 + h1));
H_value->push_back(sqrt(factor2) * d2k2_de01theta1[h1]);
for (int h2 = 0; h2 < 3; h2++) {
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m0 * 3 + h1,
m0 * 3 + h2));
H_value->push_back(-sqrt(factor1) * -d2k1_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m0 * 3 + h1,
m1 * 3 + h2));
H_value->push_back(-sqrt(factor1) * d2k1_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m1 * 3 + h1,
m0 * 3 + h2));
H_value->push_back(sqrt(factor1) * -d2k1_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m1 * 3 + h1,
m1 * 3 + h2));
H_value->push_back(sqrt(factor1) * d2k1_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m0 * 3 + h1,
m0 * 3 + h2));
H_value->push_back(-sqrt(factor2) * -d2k2_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m0 * 3 + h1,
m1 * 3 + h2));
H_value->push_back(-sqrt(factor2) * d2k2_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m1 * 3 + h1,
m0 * 3 + h2));
H_value->push_back(sqrt(factor2) * -d2k2_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m1 * 3 + h1,
m1 * 3 + h2));
H_value->push_back(sqrt(factor2) * d2k2_de01e01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m0 * 3 + h1,
m0 * 3 + h2));
H_value->push_back(-sqrt(factor3) * -d2psi_d01d01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m0 * 3 + h1,
m1 * 3 + h2));
H_value->push_back(-sqrt(factor3) * d2psi_d01d01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m1 * 3 + h1,
m0 * 3 + h2));
H_value->push_back(sqrt(factor3) * -d2psi_d01d01(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m1 * 3 + h1,
m1 * 3 + h2));
H_value->push_back(sqrt(factor3) * d2psi_d01d01(h1, h2));
}
// stiffness derivatives
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m0 * 3 + h1,
uistiffoffset + 1 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df1_dkb * dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m1 * 3 + h1,
uistiffoffset + 1 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df1_dkb * dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m0 * 3 + h1,
uistiffoffset + 1 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df2_dkb * dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m1 * 3 + h1,
uistiffoffset + 1 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df2_dkb * dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m0 * 3 + h1,
uistiffoffset + 2 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df3_dkt * dpsi_dx[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m1 * 3 + h1,
uistiffoffset + 2 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df3_dkt * dpsi_dx[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m0 * 3 + h1,
uistiffoffset + 3 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df1_dw * dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m1 * 3 + h1,
uistiffoffset + 3 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df1_dw * dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m0 * 3 + h1,
uistiffoffset + 3 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df2_dw * dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m1 * 3 + h1,
uistiffoffset + 3 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df2_dw * dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m0 * 3 + h1,
uistiffoffset + 3 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df3_dw * dpsi_dx[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m1 * 3 + h1,
uistiffoffset + 3 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df3_dw * dpsi_dx[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m0 * 3 + h1,
uistiffoffset + 4 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df1_dt * dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount,
m1 * 3 + h1,
uistiffoffset + 4 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df1_dt * dk11[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m0 * 3 + h1,
uistiffoffset + 4 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df2_dt * dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1,
m1 * 3 + h1,
uistiffoffset + 4 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df2_dt * dk21[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m0 * 3 + h1,
uistiffoffset + 4 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(-df3_dt * dpsi_dx[h1]);
Hu_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2,
m1 * 3 + h1,
uistiffoffset + 4 * nsegsTotal
+ nsegsoffset + j));
Hu_value->push_back(df3_dt * dpsi_dx[h1]);
}
constexpr bool numericalHessian = false;
if (numericalHessian) {
double gradInc = 1e-6;
Eigen::Matrix3d num_d2k1_de01e01, num_d2k2_de01e01;
Eigen::Vector3d num_d2k1_de01theta1, num_d2k2_de01theta1;
Eigen::Matrix3d num_d2psi_d01d01;
double num_d2k1_dtheta1theta1, num_d2k2_dtheta1theta1;
for (int j = 0; j < 3; j++) {
Eigen::Vector3d gradIncVec(0, 0, 0);
gradIncVec[j] += gradInc;
Eigen::Vector3d _cv0
= curstate.centerlinePositions.row(incMat(inc, 0)).transpose()
+ gradIncVec;
Eigen::Vector3d _cv1
= curstate.centerlinePositions.row(incMat(inc, 1)).transpose()
+ gradIncVec;
double _theta1 = theta + gradInc;
Eigen::Vector3d ct01_v0 = (cv1 - _cv0) / (cv1 - _cv0).norm();
Eigen::Vector3d ct01_v1 = (_cv1 - cv0) / (_cv1 - cv0).norm();
Eigen::Vector3d kb_v0 = 2.0 * ct01_v0.cross(st01)
/ (1.0 + ct01_v0.dot(st01));
Eigen::Vector3d kb_v1 = 2.0 * ct01_v1.cross(st01)
/ (1.0 + ct01_v1.dot(st01));
Eigen::Vector3d cdb1_v0 = parallelTransport(curstate.directors.row(
incMat(inc, 0)),
ct01,
ct01_v0);
Eigen::Vector3d cdb1_v1 = parallelTransport(curstate.directors.row(
incMat(inc, 0)),
ct01,
ct01_v1);
Eigen::Vector3d cdb2_v0 = ct01_v0.cross(cdb1_v0);
Eigen::Vector3d cdb2_v1 = ct01_v1.cross(cdb1_v1);
Eigen::Vector3d cd1_v0 = cdb1_v0 * cos(theta)
+ cdb2_v0 * sin(theta);
Eigen::Vector3d cd1_v1 = cdb1_v1 * cos(theta)
+ cdb2_v1 * sin(theta);
Eigen::Vector3d cd1_theta1 = cdb1 * cos(_theta1)
+ cdb2 * sin(_theta1);
Eigen::Vector3d cd2_v0 = -cdb1_v0 * sin(theta)
+ cdb2_v0 * cos(theta);
Eigen::Vector3d cd2_v1 = -cdb1_v1 * sin(theta)
+ cdb2_v1 * cos(theta);
Eigen::Vector3d cd2_theta1 = -cdb1 * sin(_theta1)
+ cdb2 * cos(_theta1);
double k1_v0 = 0.5 * ((cd2_v0 + sd2).dot(kb_v0));
double k1_v1 = 0.5 * ((cd2_v1 + sd2).dot(kb_v1));
double k1_theta1 = 0.5 * ((cd2_theta1 + sd2).dot(kb));
double k2_v0 = 0.5 * ((cd1_v0 + sd1).dot(kb_v0));
double k2_v1 = 0.5 * ((cd1_v1 + sd1).dot(kb_v1));
double k2_theta1 = 0.5 * ((cd1_theta1 + sd1).dot(kb));
Eigen::Vector3d sd1t_v0 = parallelTransport(sd1, st01, ct01_v0);
Eigen::Vector3d sd1t_v1 = parallelTransport(sd1, st01, ct01_v1);
double psi_v0 = angle(sd1t, cd1_v0, ct01_v0);
double psi_v1 = angle(sd1t, cd1_v1, ct01_v1);
Eigen::Vector3d ttilde_v0 = (ct01_v0 + st01)
/ (1.0 + ct01_v0.dot(st01));
Eigen::Vector3d ttilde_v1 = (ct01_v1 + st01)
/ (1.0 + ct01_v1.dot(st01));
Eigen::Vector3d d1tilde_v0 = (cd1_v0 + sd1)
/ (1.0 + ct01_v0.dot(st01));
Eigen::Vector3d d1tilde_v1 = (cd1_v1 + sd1)
/ (1.0 + ct01_v1.dot(st01));
Eigen::Vector3d d1tilde_theta1 = (cd1_theta1 + sd1)
/ (1.0 + ct01.dot(st01));
Eigen::Vector3d d2tilde_v0 = (cd2_v0 + sd2)
/ (1.0 + ct01_v0.dot(st01));
Eigen::Vector3d d2tilde_v1 = (cd2_v1 + sd2)
/ (1.0 + ct01_v1.dot(st01));
Eigen::Vector3d d2tilde_theta1 = (cd2_theta1 + sd2)
/ (1.0 + ct01.dot(st01));
num_d2k1_de01e01.col(j) = (1.0 / (_cv1 - cv0).norm()
* (-k1_v1 * ttilde_v1
+ st01.cross(d2tilde_v1))
- dk11)
/ gradInc;
num_d2k2_de01e01.col(j) = (1.0 / (_cv1 - cv0).norm()
* (-k2_v1 * ttilde_v1
+ st01.cross(d1tilde_v1))
- dk21)
/ gradInc;
num_d2k1_de01theta1 = (1.0 / (cv1 - cv0).norm()
* (-k1_theta1 * ttilde
+ st01.cross(d2tilde_theta1))
- dk11)
/ gradInc;
num_d2k2_de01theta1 = (1.0 / (cv1 - cv0).norm()
* (-k2_theta1 * ttilde
+ st01.cross(d1tilde_theta1))
- dk21)
/ gradInc;
num_d2psi_d01d01.col(j) = (0.5 * kb_v1 / (_cv1 - cv0).norm()
- dpsi_dx)
/ gradInc;
num_d2k1_dtheta1theta1
= (0.5 * (-cdb1 * cos(_theta1) - cdb2 * sin(_theta1)).dot(kb)
- 0.5 * Dcd2.dot(kb))
/ gradInc;
num_d2k2_dtheta1theta1
= (0.5 * (-cdb1 * sin(_theta1) + cdb2 * cos(_theta1)).dot(kb)
- 0.5 * Dcd1.dot(kb))
/ gradInc;
}
int stop = 0;
}
// the second derivatives for the euler angles are actually not needed for the optimization, so the following block is commented out for performance reasons
/*
Eigen::Matrix3d dttilde, dd1tilde, dd2tilde;
dttilde.col(0) = ((rot0*st01) * (1.0 + ct01.dot(st01)) - (ct01 + st01) * ct01.dot(rot0*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dttilde.col(1) = ((rot1*st01) * (1.0 + ct01.dot(st01)) - (ct01 + st01) * ct01.dot(rot1*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dttilde.col(2) = ((rot2*st01) * (1.0 + ct01.dot(st01)) - (ct01 + st01) * ct01.dot(rot2*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dd1tilde.col(0) = ((rot0*sd1) * (1.0 + ct01.dot(st01)) - (cd1 + sd1) * ct01.dot(rot0*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dd1tilde.col(1) = ((rot1*sd1) * (1.0 + ct01.dot(st01)) - (cd1 + sd1) * ct01.dot(rot1*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dd1tilde.col(2) = ((rot2*sd1) * (1.0 + ct01.dot(st01)) - (cd1 + sd1) * ct01.dot(rot2*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dd2tilde.col(0) = ((rot0*sd2) * (1.0 + ct01.dot(st01)) - (cd2 + sd2) * ct01.dot(rot0*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dd2tilde.col(1) = ((rot1*sd2) * (1.0 + ct01.dot(st01)) - (cd2 + sd2) * ct01.dot(rot1*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
dd2tilde.col(2) = ((rot2*sd2) * (1.0 + ct01.dot(st01)) - (cd2 + sd2) * ct01.dot(rot2*st01)) / ((1.0 + ct01.dot(st01))*(1.0 + ct01.dot(st01)));
Eigen::Matrix3d d2k11_de, d2k21_de;
d2k11_de.col(0) = 1.0 / (cv1 - cv0).norm() * (-dk1e0 * ttilde - k1 * dttilde.col(0) + (rot0*st01).cross(d2tilde) + st01.cross(dd2tilde.col(0)));
d2k11_de.col(1) = 1.0 / (cv1 - cv0).norm() * (-dk1e1 * ttilde - k1 * dttilde.col(1) + (rot1*st01).cross(d2tilde) + st01.cross(dd2tilde.col(1)));
d2k11_de.col(2) = 1.0 / (cv1 - cv0).norm() * (-dk1e2 * ttilde - k1 * dttilde.col(2) + (rot2*st01).cross(d2tilde) + st01.cross(dd2tilde.col(2)));
d2k21_de.col(0) = 1.0 / (cv1 - cv0).norm() * (-dk2e0 * ttilde - k2 * dttilde.col(0) + (rot0*st01).cross(d1tilde) + st01.cross(dd1tilde.col(0)));
d2k21_de.col(1) = 1.0 / (cv1 - cv0).norm() * (-dk2e1 * ttilde - k2 * dttilde.col(1) + (rot1*st01).cross(d1tilde) + st01.cross(dd1tilde.col(1)));
d2k21_de.col(2) = 1.0 / (cv1 - cv0).norm() * (-dk2e2 * ttilde - k2 * dttilde.col(2) + (rot2*st01).cross(d1tilde) + st01.cross(dd1tilde.col(2)));
Eigen::Matrix3d dpsi_dxde;
dpsi_dxde.col(0) = 0.5*dkb0 / (cv1 - cv0).norm();
dpsi_dxde.col(1) = 0.5*dkb1 / (cv1 - cv0).norm();
dpsi_dxde.col(2) = 0.5*dkb2 / (cv1 - cv0).norm();
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, thetaoffset + j, euleroffset + i * 3 + 0));
H_value->push_back(0.5*sqrt(factor1*width) * -Dcd2.dot(dkb0));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, thetaoffset + j, euleroffset + i * 3 + 1));
H_value->push_back(0.5*sqrt(factor1*width) * -Dcd2.dot(dkb1));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, thetaoffset + j, euleroffset + i * 3 + 2));
H_value->push_back(0.5*sqrt(factor1*width) * -Dcd2.dot(dkb2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 0, thetaoffset + j));
H_value->push_back(0.5*sqrt(factor1*width) * -Dcd2.dot(dkb0));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 1, thetaoffset + j));
H_value->push_back(0.5*sqrt(factor1*width) * -Dcd2.dot(dkb1));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 2, thetaoffset + j));
H_value->push_back(0.5*sqrt(factor1*width) * -Dcd2.dot(dkb2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, thetaoffset + j, euleroffset + i * 3 + 0));
H_value->push_back(0.5*sqrt(factor2*width*width*width) * -Dcd1.dot(dkb0));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, thetaoffset + j, euleroffset + i * 3 + 1));
H_value->push_back(0.5*sqrt(factor2*width*width*width) * -Dcd1.dot(dkb1));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, thetaoffset + j, euleroffset + i * 3 + 2));
H_value->push_back(0.5*sqrt(factor2*width*width*width) * -Dcd1.dot(dkb2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 0, thetaoffset + j));
H_value->push_back(0.5*sqrt(factor2*width*width*width) * -Dcd1.dot(dkb0));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 1, thetaoffset + j));
H_value->push_back(0.5*sqrt(factor2*width*width*width) * -Dcd1.dot(dkb1));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 2, thetaoffset + j));
H_value->push_back(0.5*sqrt(factor2*width*width*width) * -Dcd1.dot(dkb2));
for (int h1 = 0; h1 < 3; h1++)
{
for (int h2 = 0; h2 < 3; h2++)
{
// TODO: not sure about signs, check again
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, m0 * 3 + h1, euleroffset + i * 3 + h2));
H_value->push_back(-sqrt(factor1*width) * -d2k11_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + h2, m0 * 3 + h1));
H_value->push_back(-sqrt(factor1*width) * -d2k11_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, m1 * 3 + h1, euleroffset + i * 3 + h2));
H_value->push_back(sqrt(factor1*width) * -d2k11_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + h2, m1 * 3 + h1));
H_value->push_back(sqrt(factor1*width) * -d2k11_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, m0 * 3 + h1, euleroffset + i * 3 + h2));
H_value->push_back(-sqrt(factor2*width*width*width) * -d2k21_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + h2, m0 * 3 + h1));
H_value->push_back(-sqrt(factor2*width*width*width) * -d2k21_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, m1 * 3 + h1, euleroffset + i * 3 + h2));
H_value->push_back(sqrt(factor2*width*width*width) * -d2k21_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + h2, m1 * 3 + h1));
H_value->push_back(sqrt(factor2*width*width*width) * -d2k21_de(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, m0 * 3 + h1, euleroffset + i * 3 + h2));
H_value->push_back(-sqrt(factor3*width) * -dpsi_dxde(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + h2, m0 * 3 + h1));
H_value->push_back(-sqrt(factor3*width) * -dpsi_dxde(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, m1 * 3 + h1, euleroffset + i * 3 + h2));
H_value->push_back(sqrt(factor3*width) * -dpsi_dxde(h1, h2));
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + h2, m1 * 3 + h1));
H_value->push_back(sqrt(factor3*width) * -dpsi_dxde(h1, h2));
}
}
Eigen::Matrix3d rot00 = eulerRotation(0, 0, 0, 0, 0);
Eigen::Matrix3d rot01 = eulerRotation(0, 0, 0, 0, 1);
Eigen::Matrix3d rot02 = eulerRotation(0, 0, 0, 0, 2);
Eigen::Matrix3d rot11 = eulerRotation(0, 0, 0, 1, 1);
Eigen::Matrix3d rot12 = eulerRotation(0, 0, 0, 1, 2);
Eigen::Matrix3d rot22 = eulerRotation(0, 0, 0, 2, 2);
Eigen::Vector3d dkb00 = 2 * ((ct01.cross(rot00*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot0*st01) * ct01.dot(rot0*st01) - ct01.cross(rot0*st01) * (1 + ct01.dot(rot0*st01)) - ct01.cross(st01) * ct01.dot(rot00*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot0*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot0 * st01))) * -2 * ct01.dot(rot0*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb01 = 2 * ((ct01.cross(rot01*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot0*st01) * ct01.dot(rot1*st01) - ct01.cross(rot1*st01) * (1 + ct01.dot(rot0*st01)) - ct01.cross(st01) * ct01.dot(rot01*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot0*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot0 * st01))) * -2 * ct01.dot(rot1*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb02 = 2 * ((ct01.cross(rot02*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot0*st01) * ct01.dot(rot2*st01) - ct01.cross(rot2*st01) * (1 + ct01.dot(rot0*st01)) - ct01.cross(st01) * ct01.dot(rot02*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot0*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot0 * st01))) * -2 * ct01.dot(rot2*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb10 = 2 * ((ct01.cross(rot01*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot1*st01) * ct01.dot(rot0*st01) - ct01.cross(rot0*st01) * (1 + ct01.dot(rot1*st01)) - ct01.cross(st01) * ct01.dot(rot01*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot1*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot1 * st01))) * -2 * ct01.dot(rot0*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb11 = 2 * ((ct01.cross(rot11*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot1*st01) * ct01.dot(rot1*st01) - ct01.cross(rot1*st01) * (1 + ct01.dot(rot1*st01)) - ct01.cross(st01) * ct01.dot(rot11*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot1*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot1 * st01))) * -2 * ct01.dot(rot1*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb12 = 2 * ((ct01.cross(rot12*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot1*st01) * ct01.dot(rot2*st01) - ct01.cross(rot2*st01) * (1 + ct01.dot(rot1*st01)) - ct01.cross(st01) * ct01.dot(rot12*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot1*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot1 * st01))) * -2 * ct01.dot(rot2*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb20 = 2 * ((ct01.cross(rot02*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot2*st01) * ct01.dot(rot0*st01) - ct01.cross(rot0*st01) * (1 + ct01.dot(rot2*st01)) - ct01.cross(st01) * ct01.dot(rot02*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot2*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot2 * st01))) * -2 * ct01.dot(rot0*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb21 = 2 * ((ct01.cross(rot12*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot2*st01) * ct01.dot(rot1*st01) - ct01.cross(rot1*st01) * (1 + ct01.dot(rot2*st01)) - ct01.cross(st01) * ct01.dot(rot12*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot2*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot2 * st01))) * -2 * ct01.dot(rot1*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
Eigen::Vector3d dkb22 = 2 * ((ct01.cross(rot22*st01) * (1 + ct01.dot(st01)) + ct01.cross(rot2*st01) * ct01.dot(rot2*st01) - ct01.cross(rot2*st01) * (1 + ct01.dot(rot2*st01)) - ct01.cross(st01) * ct01.dot(rot22*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01)))
+ ((ct01.cross(rot2*st01) * (1 + ct01.dot(st01)) - ct01.cross(st01) * (ct01.dot(rot2 * st01))) * -2 * ct01.dot(rot2*st01)) / ((1 + ct01.dot(st01))*(1 + ct01.dot(st01))*(1 + ct01.dot(st01))));
double dk1e00 = 0.5 * ((rot00 * sd2).dot(kb) + (rot0 * sd2).dot(dkb0) + (rot0*sd2).dot(dkb0) + (cd2 + sd2).dot(dkb00));
double dk1e01 = 0.5 * ((rot01 * sd2).dot(kb) + (rot1 * sd2).dot(dkb0) + (rot1*sd2).dot(dkb0) + (cd2 + sd2).dot(dkb01));
double dk1e02 = 0.5 * ((rot02 * sd2).dot(kb) + (rot2 * sd2).dot(dkb0) + (rot2*sd2).dot(dkb0) + (cd2 + sd2).dot(dkb02));
double dk1e10 = 0.5 * ((rot01 * sd2).dot(kb) + (rot0 * sd2).dot(dkb1) + (rot0*sd2).dot(dkb1) + (cd2 + sd2).dot(dkb10));
double dk1e11 = 0.5 * ((rot11 * sd2).dot(kb) + (rot1 * sd2).dot(dkb1) + (rot1*sd2).dot(dkb1) + (cd2 + sd2).dot(dkb11));
double dk1e12 = 0.5 * ((rot12 * sd2).dot(kb) + (rot2 * sd2).dot(dkb1) + (rot2*sd2).dot(dkb1) + (cd2 + sd2).dot(dkb12));
double dk1e20 = 0.5 * ((rot02 * sd2).dot(kb) + (rot0 * sd2).dot(dkb2) + (rot0*sd2).dot(dkb2) + (cd2 + sd2).dot(dkb20));
double dk1e21 = 0.5 * ((rot12 * sd2).dot(kb) + (rot1 * sd2).dot(dkb2) + (rot1*sd2).dot(dkb2) + (cd2 + sd2).dot(dkb21));
double dk1e22 = 0.5 * ((rot22 * sd2).dot(kb) + (rot2 * sd2).dot(dkb2) + (rot2*sd2).dot(dkb2) + (cd2 + sd2).dot(dkb22));
double dk2e00 = 0.5 * ((rot00 * sd1).dot(kb) + (rot0 * sd1).dot(dkb0) + (rot0*sd1).dot(dkb0) + (cd1 + sd1).dot(dkb00));
double dk2e01 = 0.5 * ((rot01 * sd1).dot(kb) + (rot1 * sd1).dot(dkb0) + (rot1*sd1).dot(dkb0) + (cd1 + sd1).dot(dkb01));
double dk2e02 = 0.5 * ((rot02 * sd1).dot(kb) + (rot2 * sd1).dot(dkb0) + (rot2*sd1).dot(dkb0) + (cd1 + sd1).dot(dkb02));
double dk2e10 = 0.5 * ((rot01 * sd1).dot(kb) + (rot0 * sd1).dot(dkb1) + (rot0*sd1).dot(dkb1) + (cd1 + sd1).dot(dkb10));
double dk2e11 = 0.5 * ((rot11 * sd1).dot(kb) + (rot1 * sd1).dot(dkb1) + (rot1*sd1).dot(dkb1) + (cd1 + sd1).dot(dkb11));
double dk2e12 = 0.5 * ((rot12 * sd1).dot(kb) + (rot2 * sd1).dot(dkb1) + (rot2*sd1).dot(dkb1) + (cd1 + sd1).dot(dkb12));
double dk2e20 = 0.5 * ((rot02 * sd1).dot(kb) + (rot0 * sd1).dot(dkb2) + (rot0*sd1).dot(dkb2) + (cd1 + sd1).dot(dkb20));
double dk2e21 = 0.5 * ((rot12 * sd1).dot(kb) + (rot1 * sd1).dot(dkb2) + (rot1*sd1).dot(dkb2) + (cd1 + sd1).dot(dkb21));
double dk2e22 = 0.5 * ((rot22 * sd1).dot(kb) + (rot2 * sd1).dot(dkb2) + (rot2*sd1).dot(dkb2) + (cd1 + sd1).dot(dkb22));
// TODO: signs?
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 0, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor1*width) * dk1e00);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 0, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor1*width) * dk1e01);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 0, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor1*width) * dk1e02);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 1, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor1*width) * dk1e10);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 1, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor1*width) * dk1e11);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 1, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor1*width) * dk1e12);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 2, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor1*width) * dk1e20);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 2, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor1*width) * dk1e21);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount, euleroffset + i * 3 + 2, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor1*width) * dk1e22);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 0, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e00);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 0, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e01);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 0, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e02);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 1, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e10);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 1, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e11);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 1, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e12);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 2, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e20);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 2, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e21);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 1, euleroffset + i * 3 + 2, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor2*width*width*width) * dk2e22);
Eigen::Vector3d dsd1t0 = parallelDerivative(sd1, st01, ct01, rot0);
Eigen::Vector3d dsd1t1 = parallelDerivative(sd1, st01, ct01, rot1);
Eigen::Vector3d dsd1t2 = parallelDerivative(sd1, st01, ct01, rot2);
double dpsi0 = ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t0);
double dpsi1 = ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t1);
double dpsi2 = ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t2);
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2, euleroffset + i * 3 + 0, -sqrt(factor3*width) * dpsi0));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2, euleroffset + i * 3 + 1, -sqrt(factor3*width) * dpsi1));
J.push_back(Eigen::Triplet<double>(roffset + incCount + 2, euleroffset + i * 3 + 2, -sqrt(factor3*width) * dpsi2));
Eigen::Vector3d dsd1t00 = parallelSecondDerivative(sd1, st01, ct01, 0, 0);
Eigen::Vector3d dsd1t01 = parallelSecondDerivative(sd1, st01, ct01, 0, 1);
Eigen::Vector3d dsd1t02 = parallelSecondDerivative(sd1, st01, ct01, 0, 2);
Eigen::Vector3d dsd1t10 = parallelSecondDerivative(sd1, st01, ct01, 1, 0);
Eigen::Vector3d dsd1t11 = parallelSecondDerivative(sd1, st01, ct01, 1, 1);
Eigen::Vector3d dsd1t12 = parallelSecondDerivative(sd1, st01, ct01, 1, 2);
Eigen::Vector3d dsd1t20 = parallelSecondDerivative(sd1, st01, ct01, 2, 0);
Eigen::Vector3d dsd1t21 = parallelSecondDerivative(sd1, st01, ct01, 2, 1);
Eigen::Vector3d dsd1t22 = parallelSecondDerivative(sd1, st01, ct01, 2, 2);
double dpsi00 = ((dsd1t0 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t0) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t00);
double dpsi01 = ((dsd1t1 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t0) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t01);
double dpsi02 = ((dsd1t2 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t0) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t02);
double dpsi10 = ((dsd1t0 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t1) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t10);
double dpsi11 = ((dsd1t1 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t1) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t11);
double dpsi12 = ((dsd1t2 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t1) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t12);
double dpsi20 = ((dsd1t0 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t2) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t20);
double dpsi21 = ((dsd1t1 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t2) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t21);
double dpsi22 = ((dsd1t2 / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t2) + ((sd1t / sd1t.norm()).cross(ct01) / sd1t.norm()).dot(dsd1t22);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 0, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor3*width) * dpsi00);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 0, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor3*width) * dpsi01);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 0, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor3*width) * dpsi02);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 1, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor3*width) * dpsi10);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 1, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor3*width) * dpsi11);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 1, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor3*width) * dpsi12);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 2, euleroffset + i * 3 + 0));
H_value->push_back(-sqrt(factor3*width) * dpsi20);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 2, euleroffset + i * 3 + 1));
H_value->push_back(-sqrt(factor3*width) * dpsi21);
H_index->push_back(Eigen::Triplet<int>(roffset + incCount + 2, euleroffset + i * 3 + 2, euleroffset + i * 3 + 2));
H_value->push_back(-sqrt(factor3*width) * dpsi22);
*/
}
#endif
}
incCount += 3;
}
}
}
roffset += incCount;
if (roffset != nterms)
exit(-1);
if (Jr) {
// std::vector<Eigen::Triplet<double>> J_nonConstrained;
// std::unordered_set<int> constrainedDof_global;
// for (const auto &constrainedViDof : pJob->constrainedVertices) {
// const int vi = constrainedViDof.first;
// std::unordered_set<int> &constrainedDof_local = constrainedViDof.second;
// }
// for (auto &triplet : J) {
// }
Jr->setFromTriplets(J.begin(), J.end());
}
#ifdef COMPUTE_HESSIAN
if (computeHessian) {
Ju->resize(nterms, udofs);
Ju->setFromTriplets(Jui.begin(), Jui.end());
}
#endif
}
Eigen::Vector3d DER_leimer::getPerpendicularVector(const Eigen::Vector3d &t) const
{
Eigen::Vector3d candidate1 = Eigen::Vector3d(1, 0, 0).cross(t),
candidate2 = Eigen::Vector3d(0, 1, 0).cross(t);
return (candidate1.norm() > candidate2.norm()) ? candidate1.normalized()
: candidate2.normalized();
}
DER_leimer::RodConfig *DER_leimer::readRodGrid(
const std::shared_ptr<SimulationMesh> &pMesh,
const std::vector<int> &numVertsPerRod,
const std::vector<int> &
vInd, //index after the physVerts in the verts vector? Contains "global" indices of all centerline vertices eg at intersections all endpoints vertices get the same "global" index. Do interior centerline vertices get a global index?
const std::vector<Eigen::Vector3d> &vertices_global,
const std::vector<Eigen::Vector3d> &
binorms, //contains normals/binormals of all segments binorms=[nx0,nx1,..,nxn,ny0,ny1,..,nyn,nz0,nz1,..nzn]
const std::vector<RodParams> &rodparams,
const std::vector<Anchor> &anchors, //each anchor is defined by 9 doubles
const double decay,
double *vStiffness,
double *restVerts,
double *restNormals,
const std::vector<double> &thetas, //contains the thetas of all the segments
double *eulers)
{
if (numVertsPerRod.empty() || vertices_global.empty() || binorms.empty()) {
std::cerr << "Inappropriate input to readRod" << std::endl;
return nullptr;
}
// Compute total number of vertices in the matrix verts
const int numRods_config = numVertsPerRod.size();
const int numVerts_config = std::accumulate(numVertsPerRod.begin(), numVertsPerRod.end(), 0);
const int numSegs_config = numVerts_config - numRods_config; //Each rod has #verts-1 segments
if (!thetas.empty() && thetas.size() != numSegs_config) {
std::cerr << "The input thetas must have the size of the number of segments of the config"
<< std::endl;
return nullptr;
}
constexpr double Y = 1.0e+08;
RodConfig *ret = new RodConfig();
ret->allowSliding = false; //used for computing Eslide of the weaving geodesics
int numVertsAccum = 0;
int numSegsAccum = 0;
int numSegsAccumRest = 0;
//copy vertices
const int numVertsGlobal = vertices_global.size();
ret->vertices.resize(numVertsGlobal, 3);
for (int i = 0; i < numVertsGlobal; i++) {
for (int k = 0; k < 3; k++) {
// ret->vertices(i, k) = MAT(vertices_global, numVertsGlobal, i, k); //TODO:populate verts
ret->vertices(i, k) = vertices_global[i](k); //TODO:populate verts
}
}
//constructs the rods of the grid
for (int rodIndex = 0; rodIndex < numRods_config; rodIndex++) {
const int numVertices_rod = numVertsPerRod[rodIndex]; //populate numVertsPerRod
const int numSegments_rod = numVertices_rod - 1; //assumes rod is not closed
RodState rodState;
//set velocities of centerline vertices to zero
rodState.centerlineVelocities.resize(numVertices_rod, 3);
rodState.centerlineVelocities.setZero();
rodState.directorAngVel.resize(numSegments_rod);
rodState.directorAngVel.setZero();
//populate centerlineIndices and centerlinePositions
rodState.centerlinePositions.resize(numVertices_rod, 3);
rodState.centerlineIndices.resize(numVertices_rod);
for (int vi_rod = 0; vi_rod < numVertices_rod; vi_rod++) {
const int viGlobal_rod = static_cast<int>(vInd[numVertsAccum + vi_rod]);
// - 1; //vInd 1 based?
rodState.centerlineIndices(vi_rod) = viGlobal_rod;
for (int k = 0; k < 3; k++) {
// rodState.centerlinePositions(vi_rod, k) = MAT(vertices_global,
// numVertsGlobal,
// viGlobal_rod,
// k);
rodState.centerlinePositions(vi_rod, k) = vertices_global[viGlobal_rod](k);
}
}
//set the theta of each segment to zero
rodState.thetas.resize(numSegments_rod);
rodState.thetas.setZero();
//if initial thetas are not zero copy them
if (restVerts != 0 && restNormals != 0 && !thetas.empty()) {
for (int si_rod = 0; si_rod < numSegments_rod; si_rod++) {
rodState.thetas[si_rod] = thetas[numSegsAccum + si_rod];
}
}
// TODO: Allow thetas as parameter and set when provided
/*
for (int j = 0; j < nsegs; j++)
{
ifs >> rs.thetas[j];
}
*/
//set directors,..
rodState.directors.resize(numSegments_rod, 3);
rodState.matDirNormal.resize(numSegments_rod, 3);
rodState.matDirBinormal.resize(numSegments_rod, 3);
for (int si_rod = 0; si_rod < numSegments_rod; si_rod++) {
const Eigen::Vector3d v0 = rodState.centerlinePositions.row(si_rod);
const Eigen::Vector3d v1 = rodState.centerlinePositions.row(
(si_rod + 1) % numVertices_rod); //the % numVertices_rod is not needed
const Eigen::Vector3d e = (v1 - v0).normalized();
// rodState.directors.row(si_rod) = getPerpendicularVector(e);
rodState.directors.row(si_rod) = Eigen::Vector3d(0, 0, 1);
// rodState.directors.row(si_rod) = binorms[si_rod];
// for (int k = 0; k < 3; k++) {
// const auto debug_value = MAT(binorms, numSegs_config, si_rod + numSegsAccum, k);
// rodState.directors(si_rod, k) = binorms[si_rod](k);
// rodState.directors(si_rod, k) =segmentDirector[k] ;
// }
double dote = rodState.directors.row(si_rod).dot(e);
if (fabs(dote) > 1e-4) {
std::cout << dote << "Warning: directors not orthogonal to the centerline"
<< std::endl;
}
rodState.directors.row(si_rod)
-= dote * e.transpose(); //make director orthogonal to the centerline segment
rodState.directors.row(si_rod).normalize();
const Eigen::Vector3d &normalVec_segment = rodState.directors.row(si_rod);
Eigen::Vector3d binormalVec_segment = e.cross(normalVec_segment);
rodState.matDirNormal.row(si_rod)
= normalVec_segment * cos(rodState.thetas[si_rod])
+ binormalVec_segment
* sin(
rodState.thetas[si_rod]); //rotation of angle theta around segment vector
rodState.matDirBinormal.row(si_rod) = normalVec_segment * -sin(rodState.thetas[si_rod])
+ binormalVec_segment
* cos(rodState.thetas[si_rod]);
}
numSegsAccum += numSegments_rod;
bool quiet = false;
for (int si_rod = 0; si_rod < numSegments_rod - 1; si_rod++) {
const Eigen::Vector3d td
= parallelTransport(rodState.directors.row(si_rod),
rodState.centerlinePositions.row(si_rod + 1)
- rodState.centerlinePositions.row(si_rod),
rodState.centerlinePositions.row(si_rod + 2)
- rodState.centerlinePositions.row(si_rod + 1));
if (td.dot(rodState.directors.row(si_rod + 1)) < 0) { //checks if angle is > pi/2
if (!quiet) {
quiet = true;
std::cerr << "Warning: director angle > pi/2!" << std::endl;
}
rodState.directors.row(si_rod + 1) *= -1;
}
}
Eigen::VectorXd segmentWidths_eigen(numSegments_rod);
const double rodWidth = pMesh->elements[rodIndex].dimensions.getDim1();
for (int si_rod = 0; si_rod < numSegments_rod; si_rod++) {
segmentWidths_eigen[si_rod] = rodWidth;
}
Eigen::VectorXi segColors(numSegments_rod);
segColors.setZero();
if (numVertices_rod >= 2) {
//set rod parameters
RodParams params;
if (!rodparams.empty()) { //set input rod parameters
params = rodparams[rodIndex];
// params.thickness = MAT(rodparams, 8, 0, rodIndex);
// params.kstretching = MAT(rodparams, 8, 1, rodIndex);
// params.kbending = MAT(rodparams, 8, 2, rodIndex);
// params.ktwist = MAT(rodparams, 8, 3, rodIndex);
// params.kanchorpos = MAT(rodparams, 8, 4, rodIndex);
// params.kanchordir = MAT(rodparams, 8, 5, rodIndex);
// params.rho = MAT(rodparams, 8, 6, rodIndex);
// params.restlength = MAT(rodparams, 8, 7, rodIndex);
} else { //set defualt rod parameters
params.thickness = 0.001;
params.kstretching = Y;
params.kbending = Y;
params.ktwist = Y / 3.0;
params.kanchorpos = 1e4;
params.kanchordir = 1e4;
params.rho = 1.0;
params.restlength = -1;
}
Rod *r;
//Recomputes the same if an initial state is provided
if (restVerts != 0 && restNormals != 0) {
//Creates a RodState holding the the state defined in restVerts and restNormals
RodState startState;
//sets centerline positions,indinces,velocities=0
startState.centerlinePositions.resize(numVertices_rod, 3);
startState.centerlineIndices.resize(numVertices_rod);
for (int vi_rod = 0; vi_rod < numVertices_rod; vi_rod++) {
startState.centerlineIndices(vi_rod) = static_cast<int>(
vInd[numVertsAccum + vi_rod])
- 1;
int j = startState.centerlineIndices(vi_rod);
for (int k = 0; k < 3; k++) {
startState.centerlinePositions(vi_rod, k) = MAT(restVerts,
numVertsGlobal,
j,
k);
}
}
startState.centerlineVelocities.resize(numVertices_rod, 3);
startState.centerlineVelocities.setZero();
//sets directors(normal vector to segment),matDirN,mat
startState.directors.resize(numSegments_rod, 3);
startState.matDirNormal.resize(numSegments_rod, 3);
startState.matDirBinormal.resize(numSegments_rod, 3);
for (int si_rod = 0; si_rod < numSegments_rod; si_rod++) {
for (int k = 0; k < 3; k++) {
startState.directors(si_rod, k) = MAT(restNormals,
numSegs_config,
si_rod + numSegsAccumRest,
k);
}
Eigen::Vector3d v0 = startState.centerlinePositions.row(si_rod);
Eigen::Vector3d v1 = startState.centerlinePositions.row((si_rod + 1)
% numVertices_rod);
Eigen::Vector3d e = v1 - v0;
e /= e.norm();
double dote = startState.directors.row(si_rod).dot(e);
startState.directors.row(si_rod) -= dote * e.transpose();
startState.directors.row(si_rod) /= startState.directors.row(si_rod).norm();
Eigen::Vector3d dirVec = startState.directors.row(si_rod);
startState.matDirBinormal.row(si_rod) = dirVec.cross(e);
startState.matDirNormal.row(si_rod) = startState.directors.row(si_rod);
}
numSegsAccumRest += numSegments_rod;
bool quiet = false;
for (int j = 0; j < numSegments_rod - 1; j++) {
Eigen::Vector3d td
= parallelTransport(startState.directors.row(j),
startState.centerlinePositions.row(j + 1)
- startState.centerlinePositions.row(j),
startState.centerlinePositions.row(j + 2)
- startState.centerlinePositions.row(j + 1));
if (td.dot(startState.directors.row(j + 1)) < 0) {
if (!quiet) {
quiet = true;
std::cerr << "Warning: director angle > pi/2!" << std::endl;
}
startState.directors.row(j + 1) *= -1;
}
}
startState.thetas.resize(numSegments_rod);
startState.thetas.setZero();
startState.directorAngVel.resize(numSegments_rod);
startState.directorAngVel.setZero();
r = new Rod(startState, segmentWidths_eigen, params, false);
r->curState = rodState;
} else { //if no initial state is given as input
r = new Rod(rodState, segmentWidths_eigen, params, false);
}
r->setVisibilityState(Rod::RodVisibilityState::RS_VISIBLE);
r->segColors = segColors;
r->vSegParams.resize(
numVertices_rod,
4); //why is this per vertex and not per rod?Probably the last element is never accessed
if (vStiffness != 0) {
for (int vi_rod = 0; vi_rod < numVertices_rod; vi_rod++) {
for (int k = 0; k < 4; k++) {
r->vSegParams(vi_rod, k) = MAT(vStiffness,
numVerts_config,
numVertsAccum + vi_rod,
k);
}
}
} else {
//assign to the segment the parameters of the rod
for (int vi_rod = 0; vi_rod < numVertices_rod; vi_rod++) {
r->vSegParams(vi_rod, 0)
= r->params
.kstretching; //MAT(rodparams, 8, 1, rodIndex); //stretching stiffness
r->vSegParams(vi_rod, 1)
= r->params.kbending; //MAT(rodparams, 8, 2, rodIndex); //bend stiffness
r->vSegParams(vi_rod, 2)
= r->params.ktwist; //MAT(rodparams, 8, 3, rodIndex); //twist stiffness
// r->vSegParams(vi_rod, 3) = MAT(rodparams,
// 8,
// 7,
// rodIndex); //rest length. this doesnt make any sense
Eigen::Vector3d v0 = r->startState.centerlinePositions.row(vi_rod);
Eigen::Vector3d v1 = r->startState.centerlinePositions.row((vi_rod + 1)
% numVertices_rod);
r->vSegParams(vi_rod, 3) = (v1 - v0).norm();
}
}
ret->rods.push_back(r);
} else {
std::cerr << "Rod with only " << numVertices_rod << " vertices detected" << std::endl;
exit(-1);
}
numVertsAccum += numVertices_rod;
}
//ret->initWeave(); // initializes constraints more intelligently
//parse anchors
// textOutput = true;
// const int numAnchors = anchors.size() / 9;
// for (int anchorIndex = 0; anchorIndex < numAnchors; anchorIndex++) {
// Anchor anchor;
// anchor.rod = static_cast<int>(MAT(anchors, numAnchors, anchorIndex, 0) + 0.5) + indexOffset;
// anchor.seg = static_cast<int>(MAT(anchors, numAnchors, anchorIndex, 1) + 0.5) + indexOffset;
// if (anchor.rod < 0 || anchor.rod >= ret->numRods()) {
// std::cerr << "Bad rod in anchor " << anchorIndex << std::endl;
// exit(-1);
// }
// if (anchor.seg < 0 || anchor.seg >= ret->rods[anchor.rod]->numSegments()) {
// std::cerr << "Bad segment in anchor " << anchorIndex << std::endl;
// std::cerr << "Segment: " << anchor.seg << "/" << ret->rods[anchor.rod]->numSegments()
// << std::endl;
// exit(-1);
// }
// anchor.bary = MAT(anchors, numAnchors, anchorIndex, 2);
// anchor.position = Eigen::Vector3d(MAT(anchors, numAnchors, anchorIndex, 3),
// MAT(anchors, numAnchors, anchorIndex, 4),
// MAT(anchors, numAnchors, anchorIndex, 5));
// anchor.normal = Eigen::Vector3d(MAT(anchors, numAnchors, anchorIndex, 6),
// MAT(anchors, numAnchors, anchorIndex, 7),
// MAT(anchors, numAnchors, anchorIndex, 8));
// anchor.normal.normalize();
// anchor.ratio = 0.0;
// if (textOutput) {
// std::cout << "Anchor Nr. " << anchorIndex << std::endl;
// std::cout << "\tRod: " << anchor.rod << std::endl;
// std::cout << "\tSeg: " << anchor.seg << std::endl;
// std::cout << "\tBary: " << anchor.bary << std::endl;
// std::cout << "\tPos: " << anchor.position[0] << " " << anchor.position[1] << " "
// << anchor.position[2] << " " << std::endl;
// std::cout << "\tNormal: " << anchor.normal[0] << " " << anchor.normal[1] << " "
// << anchor.normal[2] << " " << std::endl;
// }
// ret->addAnchor(anchor);
// }
for (const Anchor &a : anchors) {
ret->addAnchor(a);
}
ret->decay = decay;
// find and add intersections
std::vector<int> numberOfIncidentSegments(numVertsGlobal, 0);
int numIntersections = 0;
for (int vi_config = 0; vi_config < numVerts_config; vi_config++) {
const int viGlobal = static_cast<int>(vInd[vi_config] + 0.5) /*+ indexOffset*/;
numberOfIncidentSegments[viGlobal] += 1;
if (numberOfIncidentSegments[viGlobal] == 2)
numIntersections++;
}
std::vector<int> globalViToIntersectionIndex(numVertsGlobal, -1);
for (int globalVi = 0; globalVi < numVertsGlobal; globalVi++) {
if (numberOfIncidentSegments[globalVi] > 1) {
Intersection intersection;
intersection.rotationMatrix = Eigen::Matrix3d::Identity();
intersection.intersectionIndex = ret->numIntersections();
if (eulers != 0) {
for (int mi = 0; mi < 9; mi++) {
intersection.rotationMatrix(mi) = MAT(eulers,
numIntersections,
intersection.intersectionIndex,
mi);
}
}
intersection.globalVi = globalVi;
intersection.rodSegmentIndices.resize(numberOfIncidentSegments[globalVi],
2); //matrix: #incident rods of global vertex,2
ret->intersections.push_back(intersection);
globalViToIntersectionIndex[globalVi] = intersection.intersectionIndex;
}
}
//stoped here
//populates rodSegmentIndices for each intersection
std::vector<int> intersectionRowCounter(numIntersections, 0);
for (int ri_config = 0; ri_config < numRods_config; ri_config++) {
int numVerts_rod = numVertsPerRod[ri_config];
for (int vi_rod = 0; vi_rod < numVerts_rod; vi_rod++) {
int globalVi_rod = ret->rods[ri_config]->curState.centerlineIndices(vi_rod);
if (numberOfIncidentSegments[globalVi_rod] > 1) {
Intersection &intersection
= ret->intersections[globalViToIntersectionIndex[globalVi_rod]];
intersection.rodSegmentIndices.row(
intersectionRowCounter[intersection.intersectionIndex])
<< ri_config,
vi_rod;
intersectionRowCounter[intersection.intersectionIndex]++;
}
}
}
return ret;
}
double DER_leimer::lineSearchGrid(RodConfig &config,
const Eigen::VectorXd &update,
const SimParams &params,
const Eigen::VectorXd &externalForces)
{
double t = 1;
double c1 = 0.1;
double c2 = 0.9;
double alpha = 0;
double infinity = 1e8;
double beta = infinity;
Eigen::VectorXd r;
Eigen::SparseMatrix<double> J;
double gravityPE;
Eigen::VectorXd gravityForces;
double externalForcesPE = 0;
rAndJGrid(config, r, &J, gravityPE, gravityForces, params, externalForcesPE, externalForces);
Eigen::VectorXd dE;
Eigen::VectorXd newdE;
std::vector<RodState> start;
std::vector<Intersection> startI;
Eigen::MatrixXd startV = config.vertices;
for (int i = 0; i < config.numRods(); i++) {
start.push_back(config.rods[i]->curState);
}
startI = config.intersections;
double orig = 0.5 * r.transpose() * r + gravityPE /*- externalForces.dot(update)*/;
dE = J.transpose() * r + gravityForces /*- externalForces*/;
double deriv = -dE.dot(update);
assert(deriv <= 0);
// std::cout << "Starting line search, original energy " << orig << ", descent magnitude " << deriv
// << std::endl;
int curIter = 0;
while (true) {
int nrods = config.numRods();
int dofoffset = config.numVertices() * 3;
for (int i = 0; i < config.numVertices(); i++) {
// update global vertex positions, but don't propagate the update to the centerlines yet
config.vertices.row(i) = startV.row(i) - t * update.segment<3>(3 * i).transpose();
}
for (int ri_config = 0; ri_config < nrods; ri_config++) {
int nverts = config.rods[ri_config]->numVertices();
int nsegs = config.rods[ri_config]->numSegments();
for (int i = 0; i < nsegs; i++) {
int m1 = config.rods[ri_config]->curState.centerlineIndices[i];
int m2 = config.rods[ri_config]->curState.centerlineIndices[(i + 1) % nverts];
Eigen::Vector3d oldv1 = start[ri_config].centerlinePositions.row(i);
Eigen::Vector3d oldv2 = start[ri_config].centerlinePositions.row((i + 1) % nverts);
Eigen::Vector3d v1 = oldv1 - t * update.segment<3>(3 * m1);
Eigen::Vector3d v2 = oldv2 - t * update.segment<3>(3 * m2);
config.rods[ri_config]->curState.directors.row(i)
= parallelTransport(start[ri_config].directors.row(i), oldv2 - oldv1, v2 - v1);
}
for (int i = 0; i < nverts; i++) // Update centerlines
{
//config.rods[rod]->curState.centerline.row(i) = start[rod].centerline.row(i) - t * update.segment<3>(dofoffset + 3 * i).transpose();
int m = config.rods[ri_config]->curState.centerlineIndices[i];
config.rods[ri_config]->curState.centerlinePositions.row(i) = config.vertices.row(m);
}
for (int i = 0; i < nsegs; i++) {
config.rods[ri_config]->curState.thetas[i] = start[ri_config].thetas[i]
- t * update[dofoffset + i];
Eigen::Vector3d p1 = start[ri_config].centerlinePositions.row(i);
Eigen::Vector3d p2 = start[ri_config].centerlinePositions.row((i + 1) % nverts);
Eigen::Vector3d t01 = (p2 - p1) / (p2 - p1).norm();
Eigen::Vector3d db11 = config.rods[ri_config]->curState.directors.row(i);
Eigen::Vector3d db21 = t01.cross(db11);
Eigen::Vector3d d1 = db11 * cos(config.rods[ri_config]->curState.thetas[i])
+ db21 * sin(config.rods[ri_config]->curState.thetas[i]);
Eigen::Vector3d d2 = d1.cross(t01);
config.rods[ri_config]->curState.matDirNormal.row(i) = d1.transpose();
config.rods[ri_config]->curState.matDirBinormal.row(i) = -d2.transpose();
}
dofoffset += nsegs;
}
for (int i = 0; i < config.numIntersections(); i++) {
double e0 = t * update[dofoffset + 3 * i];
double e1 = t * update[dofoffset + 3 * i + 1];
double e2 = t * update[dofoffset + 3 * i + 2];
Eigen::Matrix3d rotmat;
rotmat << cos(e0) * cos(e2), cos(e0) * sin(e2), -sin(e0),
sin(e1) * sin(e0) * cos(e2) - cos(e1) * sin(e2),
sin(e1) * sin(e0) * sin(e2) + cos(e1) * cos(e2), cos(e0) * sin(e1),
cos(e1) * sin(e0) * cos(e2) + sin(e1) * sin(e2),
cos(e1) * sin(e0) * sin(e2) - sin(e1) * cos(e2), cos(e0) * cos(e1);
config.intersections[i].rotationMatrix = rotmat * startI[i].rotationMatrix;
}
rAndJGrid(config, r, &J, gravityPE, gravityForces, params, externalForcesPE, externalForces);
double newenergy = 0.5 * r.transpose() * r + gravityPE /*- externalForces.dot(t * update)*/;
const Eigen::VectorXd internalForces = J.transpose() * r;
newdE = J.transpose() * r + gravityForces /*- externalForces*/;
// std::cout << "force residual:" << newdE.squaredNorm() << std::endl;
// std::cout << config.vertices(0, 0) << " " << config.vertices(0, 1) << " "
// << config.vertices(0, 2) << std::endl;
// std::cout << "Trying t = " << t << ", energy now " << newenergy << std::endl;
if (t < 1e-16) {
//std::cout << "t = " << t << std::endl;
return newdE.squaredNorm();
}
if (std::isnan(newenergy) || newenergy > orig) {
beta = t;
t = 0.5 * (alpha + beta);
//if(iter >= 15)
//return newdE.squaredNorm();
} else if (-newdE.dot(update) < c2 * deriv) {
alpha = t;
if (beta == infinity) {
t = 2 * alpha;
} else {
t = 0.5 * (alpha + beta);
}
if (beta - alpha < 1e-8) {
return newdE.squaredNorm();
}
} else {
return newdE.squaredNorm();
}
curIter++;
}
}
DER_leimer::Rod::Rod(const RodState &startState,
const Eigen::VectorXd &segwidths,
RodParams &params,
bool isClosed)
: startState(startState), params(params), isClosed_(isClosed),
visState_(RodVisibilityState::RS_VISIBLE)
{
int nverts = startState.centerlinePositions.rows();
int nsegs = isClosed ? nverts : nverts - 1;
assert(startState.centerlineVelocities.rows() == nverts);
assert(startState.directors.rows() == nsegs);
assert(startState.directorAngVel.size() == nsegs);
assert(startState.thetas.size() == nsegs);
assert(segwidths.size() == nsegs);
for (int i = 0; i < nsegs; i++) {
Eigen::Vector3d v1 = startState.centerlinePositions.row(i);
Eigen::Vector3d v2 = startState.centerlinePositions.row((i + 1) % nverts);
Eigen::Vector3d e = (v2 - v1);
e /= e.norm();
double dotprod = e.dot(startState.directors.row(i));
assert(fabs(dotprod) < 1e-6);
}
curState = startState;
widths = segwidths;
initializeRestQuantities();
}
void DER_leimer::Rod::initializeRestQuantities()
{
int nverts = startState.centerlinePositions.rows();
int nsegs = isClosed() ? nverts : nverts - 1;
restlens.resize(nsegs);
for (int i = 0; i < nsegs; i++) {
Eigen::Vector3d v1 = startState.centerlinePositions.row(i).transpose();
Eigen::Vector3d v2 = startState.centerlinePositions.row((i + 1) % nverts).transpose();
double len = (v1 - v2).norm();
// std::cout << "Len : " << len << std::endl;
if (params.restlength < 0)
restlens[i] = len;
else
restlens[i] = params.restlength;
}
masses.resize(nverts);
masses.setZero();
for (int i = 0; i < nsegs; i++) {
double len = restlens[i];
double totmass = widths[i] * params.thickness * len * params.rho;
masses[i] += totmass / 2.0;
masses[(i + 1) % nverts] += totmass / 2.0;
}
momInertia.resize(nsegs);
for (int i = 0; i < nsegs; i++) {
double len = restlens[i];
double mass = widths[i] * params.thickness * len * params.rho;
momInertia[i] = mass / 12.0 * (widths[i] * widths[i] + params.thickness * params.thickness);
}
}
void DER_leimer::RodConfig::addAnchor(Anchor a)
{
assert(a.rod >= 0 && a.rod < numRods());
assert(a.seg >= 0 && a.seg < rods[a.rod]->numSegments());
assert(a.bary >= 0.0 && a.bary <= 1.0);
anchors.push_back(a);
}
int DER_leimer::RodConfig::getNumDoF() const
{
int ndofs = 0;
ndofs += 3 * numVertices();
for (int i = 0; i < numRods(); i++) {
ndofs += rods[i]->numSegments();
}
// ndofs += 2 * numConstraints();
ndofs += 3 * numIntersections(); // euler angles as DOFs for intersections
return ndofs;
}
void DER_leimer::setStructure(const std::shared_ptr<SimulationMesh> &pMesh)
{
std::cout << "This function is currently not implemented" << std::endl;
assert(false);
std::terminate();
}
Reference in New Issue View Git Blame Copy Permalink