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;
    }
  }
}
Esempio n. 2
0
//==============================================================================
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;
}
Esempio n. 4
0
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;
}
Esempio n. 5
0
//==============================================================================
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)));
}
Esempio n. 6
0
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;
}
Esempio n. 7
0
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);
    
  }
}
Esempio n. 8
0
 // 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;
 };
Esempio n. 9
0
 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;
}
Esempio n. 12
0
File: ukf.cpp Progetto: nikhil9/ukf
/* 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);
    }
}
Esempio n. 13
0
 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;
 }    
Esempio n. 14
0
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;
			}
		}
	}
}
Esempio n. 15
0
//==============================================================================
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
}
Esempio n. 16
0
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;
}
Esempio n. 17
0
 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;
 }
Esempio n. 18
0
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;
  }
}
Esempio n. 19
0
 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);
 }
Esempio n. 20
0
 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);
 };
Esempio n. 21
0
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);
    }

}
Esempio n. 24
0
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);
	}
}
Esempio n. 25
0
 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;
 }    
Esempio n. 26
0
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();
        }
    }
}
Esempio n. 27
0
//==============================================================================
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;
  }
}
Esempio n. 29
0
    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();
        }
      }
    }
Esempio n. 30
0
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;
}