void GreenStrain_LIMSolver2D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess)
{
// green strain tensor energy
Eigen::Matrix<double,2,3> S;
for(int t=0;t<mesh->Triangles->rows();t++)
{
Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);
Eigen::Matrix<double,2,3> V;
V.col(0) = A;
V.col(1) = B;
V.col(2) = C;
// hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x
Eigen::Matrix3d VTV = V.transpose()*V;
Eigen::Matrix3d MMT = MMTs.block<3,3>(0,3*t);
Eigen::Matrix<double,2,3> VMMT = V*MMT;
Eigen::Matrix3d MMTVTV = MMT*VTV;
int numElem = 0;
for(int r=0;r<6;r++)
{
S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(2,3);
S.coeffRef(r) = 1;
Eigen::Matrix<double,2,3> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT);
for(int c=r;c<6;c++)
*denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider;
}
}
}
//==============================================================================
void FreeJoint::updateLocalJacobianTimeDeriv() const
{
Eigen::Matrix<double, 6, 3> J;
J.topRows<3>() = Eigen::Matrix3d::Zero();
J.bottomRows<3>() = Eigen::Matrix3d::Identity();
const Eigen::Vector6d& positions = getPositionsStatic();
const Eigen::Vector6d& velocities = getVelocitiesStatic();
Eigen::Matrix<double, 6, 3> dJ;
dJ.topRows<3>() = math::expMapJacDot(positions.head<3>(),
velocities.head<3>()).transpose();
dJ.bottomRows<3>() = Eigen::Matrix3d::Zero();
const Eigen::Isometry3d T = mT_ChildBodyToJoint
* math::expAngular(-positions.head<3>());
mJacobianDeriv.leftCols<3>() = math::AdTJacFixed(mT_ChildBodyToJoint, dJ);
const Eigen::Matrix<double, 6, 6>& Jacobian = getLocalJacobianStatic();
mJacobianDeriv.col(3)
= -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
math::AdT(T, J.col(0)));
mJacobianDeriv.col(4)
= -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
math::AdT(T, J.col(1)));
mJacobianDeriv.col(5)
= -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
math::AdT(T, J.col(2)));
}
double GreenStrain_LIMSolver3D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
// green strain energy
double shape = 0;
Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
for(int t=0;t<mesh->Tetrahedra->rows();t++)
{
Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);
Eigen::Matrix<double,3,4> V;
V.col(0) = A;
V.col(1) = B;
V.col(2) = C;
V.col(3) = D;
Eigen::Matrix3d F = V*Ms.block<4,3>(0,3*t);
Eigen::Matrix3d E = (F.transpose()*F - I);
shape += E.squaredNorm()*Divider;
}
return shape;
}
Eigen::MatrixXd bb_overlap(
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> const & bb,
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> const & bb1, int x) {
int N = bb.cols();
int NN = bb1.cols();
Eigen::VectorXd out(N);
if (x == 1) {
for (int j = 0; j < N; j++) {
out(j) = bb_overlaphelper(bb.col(j), bb1.col(j));
}
} else {
for (int j = 0; j < N; j++) {
double maxOvrlp = 0;
for (int i = 0; i < NN; i++) {
double overlap = bb_overlaphelper(bb.col(j), bb1.col(i));
if (overlap > maxOvrlp) {
maxOvrlp = overlap;
out(j) = i + 1.0;
}
}
}
}
return out;
}
//==============================================================================
void PlanarJoint::updateLocalJacobianTimeDeriv() const
{
Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
J.block<3, 1>(3, 0) = mPlanarP.mTransAxis1;
J.block<3, 1>(3, 1) = mPlanarP.mTransAxis2;
J.block<3, 1>(0, 2) = mPlanarP.mRotAxis;
const Eigen::Matrix<double, 6, 3>& Jacobian = getLocalJacobianStatic();
const Eigen::Vector3d& velocities = getVelocitiesStatic();
mJacobianDeriv.col(0)
= -math::ad(Jacobian.col(2) * velocities[2],
math::AdT(mJointP.mT_ChildBodyToJoint
* math::expAngular(mPlanarP.mRotAxis
* -getPositionsStatic()[2]),
J.col(0)));
mJacobianDeriv.col(1)
= -math::ad(Jacobian.col(2) * velocities[2],
math::AdT(mJointP.mT_ChildBodyToJoint
* math::expAngular(mPlanarP.mRotAxis
* -getPositionsStatic()[2]),
J.col(1)));
assert(mJacobianDeriv.col(2) == Eigen::Vector6d::Zero());
assert(!math::isNan(mJacobianDeriv.col(0)));
assert(!math::isNan(mJacobianDeriv.col(1)));
}
Eigen::Matrix<double,3,3> Plane::getRotationMatrix(){
Eigen::Matrix<double,3,3> rotMat;
rotMat.col(0) = inertiaAxisX.normalized();
rotMat.col(1) = inertiaAxisY.normalized();
rotMat.col(2) = Eigen::Vector3d(normalVector[0], normalVector[1], normalVector[2]).normalized();
return rotMat;
}
inline void igl::PlanarizerShapeUp<DerivedV, DerivedF>::assembleP()
{
P.setZero(3*ni*numF);
for (int fi = 0; fi< numF; fi++)
{
// todo: this can be made faster by omitting the selector matrix
Eigen::SparseMatrix<typename DerivedV::Scalar > Sfi;
assembleSelector(fi, Sfi);
Eigen::SparseMatrix<typename DerivedV::Scalar > NSi = Ni*Sfi;
Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> Vi = NSi*Vv;
Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> CC(3,ni);
for (int i = 0; i <ni; ++i)
CC.col(i) = Vi.segment(3*i, 3);
Eigen::Matrix<typename DerivedV::Scalar, 3, 3> C = CC*CC.transpose();
// Alec: Doesn't compile
Eigen::EigenSolver<Eigen::Matrix<typename DerivedV::Scalar, 3, 3>> es(C);
// the real() is for compilation purposes
Eigen::Matrix<typename DerivedV::Scalar, 3, 1> lambda = es.eigenvalues().real();
Eigen::Matrix<typename DerivedV::Scalar, 3, 3> U = es.eigenvectors().real();
int min_i;
lambda.cwiseAbs().minCoeff(&min_i);
U.col(min_i).setZero();
Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> PP = U*U.transpose()*CC;
for (int i = 0; i <ni; ++i)
P.segment(3*ni*fi+3*i, 3) = weightsSqrt[fi]*PP.col(i);
}
}
// transforms
void transformW2F(Eigen::Matrix<double,3,4> &m,
const Eigen::Matrix<double,4,1> &trans,
const Eigen::Quaternion<double> &qrot)
{
m.block<3,3>(0,0) = qrot.toRotationMatrix();//.transpose();
m.col(3).setZero(); // make sure there's no translation
m.col(3) = -m*trans;
};
inline Eigen::Matrix<double, 1, C1>
columns_dot_product(const Eigen::Matrix<double, R1, C1>& v1,
const Eigen::Matrix<double, R2, C2>& v2) {
validate_matching_sizes(v1,v2,"columns_dot_product");
Eigen::Matrix<double, 1, C1> ret(1,v1.cols());
for (size_type j = 0; j < v1.cols(); ++j) {
ret(j) = v1.col(j).dot(v2.col(j));
}
return ret;
}
boost::shared_ptr<const Mantid::Geometry::IObject> createShape() {
Eigen::Matrix<double, 3, 3> pointsDef;
// Vector to base positioned at center of cylinder
pointsDef.col(0) = Eigen::Vector3d(-0.00101, 0.0, 0.0);
// vector to base which is radial
pointsDef.col(1) = Eigen::Vector3d(-0.00101, 0.00405, 0.0);
// vector to top positioned at center of cylinder
pointsDef.col(2) = Eigen::Vector3d(0.00101, 0.0, 0.0);
return NexusShapeFactory::createCylinder(pointsDef);
}
Pixels generateNonCoLinearPixels() {
// Add two 4 cylinders which are not CoLinear
Eigen::Matrix<double, 3, 4> pix;
pix.col(0) = Eigen::Vector3d(0, 0.1, 0);
pix.col(1) = Eigen::Vector3d(0.3, 0.6, 0.3);
pix.col(2) = Eigen::Vector3d(-0.7, -0.7, 0);
pix.col(3) = Eigen::Vector3d(1, 1.9, 0);
return pix;
}
/* Follows section 3.1, however we're using the scaled unscented transform. */
void UnscentedKalmanFilter::create_sigma_points() {
Eigen::Matrix<real_t, UKF_STATE_DIM, UKF_STATE_DIM> S;
AssertNormalized(Quaternionr(state.attitude()));
/* Add the process noise before calculating the square root. */
state_covariance.diagonal() += process_noise_covariance;
state_covariance *= UKF_DIM_PLUS_LAMBDA;
AssertPositiveDefinite(state_covariance);
/* Compute the 'square root' of the covariance matrix. */
S = state_covariance.llt().matrixL();
/* Create centre sigma point. */
sigma_points.col(0) = state;
/* For each column in S, create the two complementary sigma points. */
for(uint8_t i = 0; i < UKF_STATE_DIM; i++) {
/*
Construct error quaternions using the MRP method, equation 34 from the
Markley paper.
*/
Vector3r d_p = S.col(i).segment<3>(9);
real_t x_2 = d_p.squaredNorm();
real_t err_w = (-UKF_MRP_A * x_2 +
UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
(UKF_MRP_F_2 + x_2);
Vector3r err_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + err_w)) * d_p;
Quaternionr noise;
noise.vec() = err_xyz;
noise.w() = err_w;
Quaternionr temp;
/* Create positive sigma point. */
sigma_points.col(i+1).segment<9>(0) =
state.segment<9>(0) + S.col(i).segment<9>(0);
temp = noise * Quaternionr(state.attitude());
AssertNormalized(temp);
sigma_points.col(i+1).segment<4>(9) << temp.vec(), temp.w();
sigma_points.col(i+1).segment<12>(13) =
state.segment<12>(13) + S.col(i).segment<12>(12);
/* Create negative sigma point. */
sigma_points.col(i+1+UKF_STATE_DIM).segment<9>(0) =
state.segment<9>(0) - S.col(i).segment<9>(0);
temp = noise.conjugate() * Quaternionr(state.attitude());
AssertNormalized(temp);
sigma_points.col(i+1+UKF_STATE_DIM).segment<4>(9) <<
temp.vec(), temp.w();
sigma_points.col(i+1+UKF_STATE_DIM).segment<12>(13) =
state.segment<12>(13) - S.col(i).segment<12>(12);
}
}
inline Eigen::Matrix<double, 1, C1>
columns_dot_product(const Eigen::Matrix<double, R1, C1>& v1,
const Eigen::Matrix<double, R2, C2>& v2) {
stan::math::check_matching_sizes("columns_dot_product",
"v1", v1,
"v2", v2);
Eigen::Matrix<double, 1, C1> ret(1,v1.cols());
for (size_type j = 0; j < v1.cols(); ++j) {
ret(j) = v1.col(j).dot(v2.col(j));
}
return ret;
}
void IDIM::computeY(const MultiBody& mb, const MultiBodyConfig& mbc)
{
const std::vector<Body>& bodies = mb.bodies();
const std::vector<Joint>& joints = mb.joints();
const std::vector<int>& pred = mb.predecessors();
Eigen::Matrix<double, 6, 10> bodyFPhi;
for(int i = static_cast<int>(bodies.size()) - 1; i >= 0; --i)
{
const sva::MotionVecd& vb_i = mbc.bodyVelB[i];
Eigen::Matrix<double, 6, 10> vb_i_Phi(IMPhi(vb_i));
bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[i]);
// bodyFPhi += vb_i x* IMPhi(vb_i)
// is faster to convert each col in a ForceVecd
// than using sva::vector6ToCrossDualMatrix
for(int c = 0; c < 10; ++c)
{
bodyFPhi.col(c).noalias() +=
(vb_i.crossDual(sva::ForceVecd(vb_i_Phi.col(c)))).vector();
}
int iDofPos = mb.jointPosInDof(i);
Y_.block(iDofPos, i*10, joints[i].dof(), 10).noalias() =
mbc.motionSubspace[i].transpose()*bodyFPhi;
int j = i;
while(pred[j] != -1)
{
const sva::PTransformd& X_p_j = mbc.parentToSon[j];
// bodyFPhi = X_p_j^T bodyFPhi
// is faster to convert each col in a ForceVecd
// than using X_p_j.inv().dualMatrix()
for(int c = 0; c < 10; ++c)
{
bodyFPhi.col(c) =
X_p_j.transMul(sva::ForceVecd(bodyFPhi.col(c))).vector();
}
j = pred[j];
int jDofPos = mb.jointPosInDof(j);
if(joints[j].dof() != 0)
{
Y_.block(jDofPos, i*10, joints[j].dof(), 10).noalias() =
mbc.motionSubspace[j].transpose()*bodyFPhi;
}
}
}
}
//==============================================================================
BallJoint::BallJoint(const std::string& _name)
: MultiDofJoint(_name),
mR(Eigen::Isometry3d::Identity())
{
// Jacobian
Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
J.topRows<3>() = Eigen::Matrix3d::Identity();
mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J.col(0));
mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J.col(1));
mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J.col(2));
assert(!math::isNan(mJacobian));
// Time derivative of Jacobian is always zero
}
Eigen::VectorXd bb_overlap(Eigen::Matrix<double, 4, Eigen::Dynamic> const & bb1) {
double nBB = bb1.cols();
Eigen::VectorXd out(nBB * (nBB - 1) / 2);
int counter = 0;
for (int i = 0; i < nBB - 1; i++) {
for (int j = i + 1; j < nBB; j++) {
out(counter) = bb_overlaphelper(bb1.col(i), bb1.col(j));
counter++;
}
}
return out;
}
inline
Eigen::Matrix<fvar<T>, 1, C1>
columns_dot_product(const Eigen::Matrix<double, R1, C1>& v1,
const Eigen::Matrix<fvar<T>, R2, C2>& v2) {
stan::math::check_matching_dims("columns_dot_product",
"v1", v1,
"v2", v2);
Eigen::Matrix<fvar<T>, 1, C1> ret(1, v1.cols());
for (size_type j = 0; j < v1.cols(); ++j) {
Eigen::Matrix<double, R1, C1> ccol = v1.col(j);
Eigen::Matrix<fvar<T>, R2, C2> ccol2 = v2.col(j);
ret(0, j) = dot_product(ccol, ccol2);
}
return ret;
}
IGL_INLINE void igl::mat_min(
const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & X,
const int dim,
Eigen::Matrix<T,Eigen::Dynamic,1> & Y,
Eigen::Matrix<int,Eigen::Dynamic,1> & I)
{
assert(dim==1||dim==2);
// output size
int n = (dim==1?X.cols():X.rows());
// resize output
Y.resize(n);
I.resize(n);
// loop over dimension opposite of dim
for(int j = 0;j<n;j++)
{
typename Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Index PHONY;
typename Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Index i;
T m;
if(dim==1)
{
m = X.col(j).minCoeff(&i,&PHONY);
}else
{
m = X.row(j).minCoeff(&PHONY,&i);
}
Y(j) = m;
I(j) = i;
}
}
inline
Eigen::Matrix<T,Eigen::Dynamic,1>
col(const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>& m,
size_t j) {
validate_column_index(m,j,"col");
return m.col(j - 1);
}
void transformF2W(Eigen::Matrix<double,3,4> &m,
const Eigen::Matrix<double,4,1> &trans,
const Eigen::Quaternion<double> &qrot)
{
m.block<3,3>(0,0) = qrot.toRotationMatrix();
m.col(3) = trans.head(3);
};
void check_accelerations(const double amin_[N_MOTORS], const double amax_[N_MOTORS], const Eigen::Matrix <double,
N_SEGMENTS, N_MOTORS> & m3w_, const Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m2w_, const Eigen::Matrix <
double, N_SEGMENTS, 1> taus_)
{
#if 0
cout << "amin:\n";
for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
cout << amin_[mtr] << " ";
}
cout << endl;
cout << "amax:\n";
for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
cout << amax_[mtr] << " ";
}
cout << endl;
#endif
// Compute acceleration values at segment beginning.
Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> a_start = 2.0 * m2w_;
#if 0
cout << "a_start:\n" << a_start << endl;
#endif
// Compute acceleration values at the segment end.
Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> a_final;
for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
a_final.col(mtr) = 6.0 * (m3w_.col(mtr).cwise() * taus_) + 2.0 * m2w_.col(mtr);
}
#if 0
cout << "a_final:\n" << a_final << endl;
#endif
// Check conditions for all segments and motors.
for (int sgt = 0; sgt < N_SEGMENTS; ++sgt)
for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
if (a_start(sgt, mtr) > amax_[mtr])
BOOST_THROW_EXCEPTION(nfe_motor_acceleration_constraint_exceeded() << constraint_type(MAXIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(amax_[mtr]) << desired_value(a_start(sgt, mtr)));
else if (a_final(sgt, mtr) > amax_[mtr])
BOOST_THROW_EXCEPTION(nfe_motor_acceleration_constraint_exceeded() << constraint_type(MAXIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(amax_[mtr]) << desired_value(a_final(sgt, mtr)));
else if (a_start(sgt, mtr) < amin_[mtr])
BOOST_THROW_EXCEPTION(nfe_motor_acceleration_constraint_exceeded() << constraint_type(MINIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(amin_[mtr]) << desired_value(a_start(sgt, mtr)));
else if (a_final(sgt, mtr) < amin_[mtr])
BOOST_THROW_EXCEPTION(nfe_motor_acceleration_constraint_exceeded() << constraint_type(MINIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(amin_[mtr]) << desired_value(a_final(sgt, mtr)));
}
}
Pixels generateCoLinearPixels() {
// Assuming tubes with axis vectors (1, 0, 0) create two tubes lying along x
// axis.
Eigen::Matrix<double, 3, 4> pix;
double tube1_y = 0;
double tube2_y = 0.05;
// Add 2 cylinder positions to tube 1
for (int i = 0; i < 2; ++i)
pix.col(i) = Eigen::Vector3d(0.00202 * i, tube1_y, 0);
// Add 2 cylinder positions to tube 2
for (int i = 2; i < 4; ++i)
pix.col(i) = Eigen::Vector3d(0.00202 * i, tube2_y, 0);
return pix;
}
void
approx_relpose_generalized
(
const Eigen::Matrix<double,6,6> &w1,
const Eigen::Matrix<double,6,6> &w2,
const Eigen::Matrix<double,6,6> &w3,
const Eigen::Matrix<double,6,6> &w4,
const Eigen::Matrix<double,6,6> &w5,
const Eigen::Matrix<double,6,6> &w6,
std::vector<Eigen::Vector3d> &rsolns
)
{
const Eigen::Matrix<double,15,35> A = computeA(w1,w2,w3,w4,w5,w6);
Eigen::Matrix<double,15,35> gbA;
gbA << A.col(0),A.col(1),A.col(2),A.col(3),A.col(4),A.col(5),A.col(7),A.col(9),A.col(11),A.col(15),A.col(18),A.col(21),A.col(24),A.col(28),A.col(13),A.col(6),A.col(8),A.col(10),A.col(12),A.col(16),A.col(19),A.col(22),A.col(25),A.col(29),A.col(14),A.col(17),A.col(20),A.col(23),A.col(26),A.col(30),A.col(32),A.col(27),A.col(31),A.col(33),A.col(34);
const Eigen::Matrix<double,15,20> G = gbA.block<15,15>(0,0).lu().solve(gbA.block<15,20>(0,15));
Eigen::Matrix<double,20,20> M = Eigen::Matrix<double,20,20>::Zero();
M.block<10,20>(0,0) = -G.block<10,20>(5,0);
M(10,4) = 1;
M(11,5) = 1;
M(12,6) = 1;
M(13,7) = 1;
M(14,8) = 1;
M(15,9) = 1;
M(16,13) = 1;
M(17,14) = 1;
M(18,15) = 1;
M(19,18) = 1;
const Eigen::EigenSolver< Eigen::Matrix<double,20,20> > eigensolver(M,true);
const Eigen::EigenSolver< Eigen::Matrix<double,20,20> >::EigenvalueType evalues = eigensolver.eigenvalues();
const Eigen::EigenSolver< Eigen::Matrix<double,20,20> >::EigenvectorsType evecs = eigensolver.eigenvectors();
rsolns.clear();
rsolns.reserve(evalues.size());
for ( size_t i = 0; i < evalues.size(); i++ )
{
if ( evalues[i].imag() != 0 ) continue;
const double zsoln = evalues(i).real();
const double xsoln = evecs(16,i).real()/evecs(19,i).real();
const double ysoln = evecs(17,i).real()/evecs(19,i).real();
Eigen::Vector3d rsoln;
rsoln << xsoln, ysoln, zsoln;
rsolns.push_back(rsoln);
}
}
void cholesky_update (Eigen::Matrix<double, N, N>& L, Eigen::Matrix<double, N, 1> v) {
Eigen::JacobiRotation<double> rot;
for (int i = 0; i < N; ++i) {
rot.makeGivens(L(i,i), -v(i), &L(i,i)), v(i) = 0;
if (i < N-1) apply_jacobi_rotation(L.col(i).tail(N-i-1), v.tail(N-i-1), rot);
}
}
inline Eigen::Matrix<fvar<T>,1,C>
columns_dot_self(const Eigen::Matrix<fvar<T>,R,C>& x) {
Eigen::Matrix<fvar<T>,1,C> ret(1,x.cols());
for (size_type i = 0; i < x.cols(); i++) {
Eigen::Matrix<fvar<T>,R,1> ccol = x.col(i);
ret(0,i) = dot_self(ccol);
}
return ret;
}
void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim)
{
Eigen::Matrix<double,6,6> Ainv = A.inverse();
Eigen::Matrix<double,6,6> ABRB;
if (zDim == 6)
{
ABRB = Ainv * B * _R.llt().solve(B.transpose());
}
else {
ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose());
}
Eigen::Matrix<double,2*6,2*6> Z;
Z.block(0,0,6,6) = Ainv;
Z.block(0,6,6,6) = ABRB;
Z.block(6,0,6,6) = _Q * Ainv;
Z.block(6,6,6,6) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*6; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > 6)
break;
}
}
Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse();
Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv;
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 6; j++) {
P(i,j) = PP(i,j).real();
}
}
}
//==============================================================================
Eigen::Matrix<double, 6, 3> PlanarJoint::getLocalJacobianStatic(
const Eigen::Vector3d& _positions) const
{
Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
J.block<3, 1>(3, 0) = mPlanarP.mTransAxis1;
J.block<3, 1>(3, 1) = mPlanarP.mTransAxis2;
J.block<3, 1>(0, 2) = mPlanarP.mRotAxis;
J.leftCols<2>()
= math::AdTJacFixed(mJointP.mT_ChildBodyToJoint
* math::expAngular(mPlanarP.mRotAxis * -_positions[2]),
J.leftCols<2>());
J.col(2) = math::AdTJac(mJointP.mT_ChildBodyToJoint, J.col(2));
// Verification
assert(!math::isNan(J));
return J;
}
void GreenStrain_LIMSolver2D::computeGradient(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, Eigen::Matrix<double,Eigen::Dynamic,1>& grad)
{
// green strain energy
for(int t=0;t<mesh->Triangles->rows();t++)
{
Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);
Eigen::Matrix<double,2,3> V;
V.col(0) = A;
V.col(1) = B;
V.col(2) = C;
// jacobian(E) = 4(VMM'V'VMM' - VMM')
Eigen::Matrix<double,2,3> VMMT = V*MMTs.block<3,3>(0,3*t);
Eigen::Matrix<double,2,3> T = 4*(VMMT*V.transpose()*VMMT - VMMT);
for(int i=0;i<6;i++)
grad[TriangleVertexIdx.coeff(i,t)] += T.coeff(i)*Divider;
}
}
void dare(const Eigen::Matrix<double,xDim,xDim> &A, const Eigen::Matrix<double,xDim,uDim> &B, Eigen::Matrix<double,xDim,xDim> &P)
{
Eigen::Matrix<double,xDim,xDim> Ainv = A.inverse();
Eigen::Matrix<double,xDim,xDim> ABRB = Ainv * B * _R.llt().solve(B.transpose());
Eigen::Matrix<double,2*xDim,2*xDim> Z;
Z.block(0,0,xDim,xDim) = Ainv;
Z.block(0,xDim,xDim,xDim) = ABRB;
Z.block(xDim,0,xDim,xDim) = _Q * Ainv;
Z.block(xDim,xDim,xDim,xDim) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*xDim,2*xDim> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*xDim,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*xDim,2*xDim> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*xDim,xDim> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*xDim; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > xDim)
break;
}
}
Eigen::Matrix<std::complex<double>,xDim,xDim> U21inv = unstableEigVec.block(0,0,xDim,xDim).inverse();
Eigen::Matrix<std::complex<double>,xDim,xDim> PP = unstableEigVec.block(xDim,0,xDim,xDim) * U21inv;
for (int i = 0; i < xDim; i++) {
for (int j = 0; j < xDim; j++) {
P(i,j) = PP(i,j).real();
}
}
}
Eigen::MatrixXd bb_overlap(
Eigen::Matrix<double, 4, Eigen::Dynamic> const & bb, int n1,
Eigen::Matrix<double, 4, Eigen::Dynamic> const & bb1) {
int N = bb.cols();
int NN = n1;
if (N == 0 || NN == 0) {
N = 0;
NN = 0;
}
Eigen::MatrixXd out(N, NN);
for (int i = 0; i < N; i++) {
for (int j = 0; j < NN; j++) {
out(i, j) = bb_overlaphelper(bb.col(i), bb1.col(j));
}
}
return out;
}
