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; }