C++ Matrix4d::data方法代码示例

void igl::sort_triangles(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedI> & I)
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;
  // Put model, projection, and viewport matrices into double arrays
  Matrix4d MV;
  Matrix4d P;
  glGetDoublev(GL_MODELVIEW_MATRIX,  MV.data());
  glGetDoublev(GL_PROJECTION_MATRIX, P.data());
  if(V.cols() == 3)
  {
    Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime,4> hV;
    hV.resize(V.rows(),4);
    hV.block(0,0,V.rows(),V.cols()) = V;
    hV.col(3).setConstant(1);
    return sort_triangles(hV,F,MV,P,FF,I);
  }else
  {
    return sort_triangles(V,F,MV,P,FF,I);
  }
}

void WingGeom::UpdateDrawObj()
{
    Geom::UpdateDrawObj();

    Matrix4d attachMat;
    Matrix4d relTrans;
    attachMat = ComposeAttachMatrix();
    relTrans = attachMat;
    relTrans.affineInverse();
    relTrans.matMult( m_ModelMatrix.data() );
    relTrans.postMult( attachMat.data() );

    int nxsec = m_XSecSurf.NumXSec();
    m_XSecDrawObj_vec.resize( nxsec, DrawObj() );

    //==== Tesselate Surface ====//
    for ( int i = 0 ; i < nxsec ; i++ )
    {
        m_XSecDrawObj_vec[i].m_PntVec = m_XSecSurf.FindXSec( i )->GetDrawLines( m_TessW(), relTrans );
        m_XSecDrawObj_vec[i].m_GeomChanged = true;
    }

    m_HighlightXSecDrawObj.m_PntVec = m_XSecSurf.FindXSec( m_ActiveAirfoil )->GetDrawLines( m_TessW(), relTrans );
    m_HighlightXSecDrawObj.m_GeomChanged = true;

    double w = m_XSecSurf.FindXSec( m_ActiveAirfoil )->GetXSecCurve()->GetWidth();

    Matrix4d mat;
    m_XSecSurf.GetBasicTransformation( Z_DIR, X_DIR, XS_SHIFT_MID, false, 1.0, mat );
    mat.scale( 1.0/w );

    VspCurve crv = m_XSecSurf.FindXSec( m_ActiveAirfoil )->GetUntransformedCurve();
    crv.Transform( mat );

    vector< vec3d > pts;
    crv.Tesselate( m_TessW(), pts );

    m_CurrentXSecDrawObj.m_PntVec = pts;
    m_CurrentXSecDrawObj.m_LineWidth = 1.0;
    m_CurrentXSecDrawObj.m_LineColor = vec3d( 0.0, 0.0, 0.0 );
    m_CurrentXSecDrawObj.m_Type = DrawObj::VSP_LINES;
    m_CurrentXSecDrawObj.m_GeomChanged = true;


    VspCurve inbd = m_XSecSurf.FindXSec( m_ActiveXSec - 1 )->GetCurve();
    inbd.Transform( relTrans );

    VspCurve outbd = m_XSecSurf.FindXSec( m_ActiveXSec )->GetCurve();
    outbd.Transform( relTrans );

    BndBox iBBox, oBBox;
    inbd.GetBoundingBox( iBBox );
    outbd.GetBoundingBox( oBBox );
    oBBox.Update( iBBox );

    m_HighlightWingSecDrawObj.m_PntVec = oBBox.GetBBoxDrawLines();

}

void Data4Viewer::drawRGBView()
{
	_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);

	glMatrixMode ( GL_MODELVIEW );
	Eigen::Affine3d tmp; tmp.setIdentity();
	Matrix4d mv = btl::utility::setModelViewGLfromPrj(tmp); //mv transform X_m to X_w i.e. model coordinate to world coordinate
	glLoadMatrixd( mv.data() );
	_pKinect->_pRGBCamera->renderCameraInLocal(_pKinect->_pCurrFrame->_gRGB,  _pGL.get(),false, NULL, 0.2f, true ); //render in model coordinate
	//PRINTSTR("drawRGBView");
    return;
}

void XSecSurf::GetBasicTransformation( int pdir, int wdir, int wshift, bool flip, double w, Matrix4d &mat )
{
    double *m = mat.data();

    for ( int i = 0; i < 16; i++ )
    {
        m[i] = 0;
    }

    int prow = pdir;
    // Principal direction of base curves +Z
    m[ prow + ( Z_DIR * 4 ) ] = 1;

    int wrow = wdir;
    // Width direction of base curves +X
    m[ wrow + ( X_DIR * 4 ) ] = 1;

    // Remaining row via clever math
    int row = 3 - ( prow + wrow );

    // Cross product to ensure right handed system
    int r1, r2;
    switch( row )
    {
    case X_DIR:
        r1 = Y_DIR;
        r2 = Z_DIR;
        break;
    case Y_DIR:
        r1 = Z_DIR;
        r2 = X_DIR;
        break;
    case Z_DIR:
        r1 = X_DIR;
        r2 = Y_DIR;
        break;
    }

    int flipflag = 1;
    if ( flip )
    {
        flipflag = -1;
    }

    // Specialized cross product with known zeros.
    m[ row + ( Y_DIR * 4 ) ] = flipflag * ( m[ r1 + ( Z_DIR * 4 ) ] * m[ r2 + ( X_DIR * 4 ) ] -
                                            m[ r1 + ( X_DIR * 4 ) ] * m[ r2 + ( Z_DIR * 4 ) ] );

    // Shift in width direction if required.
    m[ wrow + 12 ] = -w * wshift / 2.0;
}

//==== Transform Control Points =====//
void VspCurve::Transform( Matrix4d & mat )
{
    curve_rotation_matrix_type rmat;
    double *mmat( mat.data() );
    curve_point_type trans;

    rmat << mmat[0], mmat[4], mmat[8],
         mmat[1], mmat[5], mmat[9],
         mmat[2], mmat[6], mmat[10];
    trans << mmat[12], mmat[13], mmat[14];

    m_Curve.rotate( rmat );
    m_Curve.translate( trans );
}

void CData4Viewer::drawDepthView(qglviewer::Camera* pCamera_)
{
	_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.01f,20.f);

	_pDepth->setRTw( Matrix3f::Identity(),Vector3f(0,0,0) );
	//_pDepth->gpuTransformToWorldCVCV();

	Matrix4f mModelView;
	_pDepth->getGLModelViewMatrix(&mModelView);
	Matrix4d mTmp = mModelView.cast<double>(); 
	pCamera_->setFromModelViewMatrix( mTmp.data() );

	_pDepth->gpuRender3DPts(_pGL.get(), _pGL->_usLevel);
	//PRINTSTR("drawDepthView");
	return;
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //cout << "constructModelmex: START" << endl;
  //END_DEBUG
  char buf[100];
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  if (isa(prhs[0],"DrakeMexPointer")) {  // then it's calling the destructor
    destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]);
    return;
  }

  const mxArray* pRBM = prhs[0];
  RigidBodyManipulator *model=NULL;

//  model->robot_name = get_strings(mxGetProperty(pRBM,0,"name"));

  const mxArray* pBodies = mxGetProperty(pRBM,0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies));

  const mxArray* pFrames = mxGetProperty(pRBM,0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));

  pm = mxGetProperty(pRBM, 0, "num_positions");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","model should have a num_positions field");
  int num_positions = static_cast<int>(*mxGetPrSafe(pm));
  model = new RigidBodyManipulator(num_positions, num_bodies, num_frames);

  for (int i=0; i<model->num_bodies; i++) {
    //DEBUG
    //cout << "constructModelmex: body " << i << endl;
    //END_DEBUG
    model->bodies[i]->body_index = i;

    pm = mxGetProperty(pBodies,i,"linkname");
    mxGetString(pm,buf,100);
    model->bodies[i]->linkname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"robotnum");
    model->bodies[i]->robotnum = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pBodies,i,"mass");
    model->bodies[i]->mass = mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"com");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->com.data(),mxGetPrSafe(pm),sizeof(double)*3);

    pm = mxGetProperty(pBodies,i,"I");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6);

    pm = mxGetProperty(pBodies,i,"position_num");
    model->bodies[i]->position_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"velocity_num");
    model->bodies[i]->velocity_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"parent");
    if (!pm || mxIsEmpty(pm))
      model->bodies[i]->parent = nullptr;
    else {
      int parent_ind = static_cast<int>(mxGetScalar(pm))-1;
      if (parent_ind >= static_cast<int>(model->bodies.size()))
        mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size());
      if (parent_ind>=0)
        model->bodies[i]->parent = model->bodies[parent_ind];
    }

    {
      mxGetString(mxGetProperty(pBodies, i, "jointname"), buf, 100);
      string jointname;
      jointname.assign(buf, strlen(buf));

      pm = mxGetProperty(pBodies, i, "Ttree");
      // todo: check that the size is 4x4
      Isometry3d Ttree;
      memcpy(Ttree.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4);

      int floating = (int) mxGetScalar(mxGetProperty(pBodies, i, "floating"));

      Eigen::Vector3d joint_axis;
      pm = mxGetProperty(pBodies, i, "joint_axis");
      memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);

      double pitch = mxGetScalar(mxGetProperty(pBodies, i, "pitch"));

      if (model->bodies[i]->hasParent()) {
        unique_ptr<DrakeJoint> joint = createJoint(jointname, Ttree, floating, joint_axis, pitch);

//        mexPrintf((model->bodies[i]->getJoint().getName() + ": " + std::to_string(model->bodies[i]->getJoint().getNumVelocities()) + "\n").c_str());

        FixedAxisOneDoFJoint* fixed_axis_one_dof_joint = dynamic_cast<FixedAxisOneDoFJoint*>(joint.get());
        if (fixed_axis_one_dof_joint != nullptr) {
          double joint_limit_min = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_min"));
//.........这里部分代码省略.........

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //cout << "constructModelmex: START" << endl;
  //END_DEBUG
  char buf[100];
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  if (isa(prhs[0],"DrakeMexPointer")) {  // then it's calling the destructor
    destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]);
    return;
  }

  const mxArray* pRBM = prhs[0];
  RigidBodyManipulator *model=NULL;

//  model->robot_name = get_strings(mxGetProperty(pRBM,0,"name"));

  const mxArray* featherstone = mxGetProperty(pRBM,0,"featherstone");
  if (!featherstone) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs", "the featherstone array is invalid");

  const mxArray* pBodies = mxGetProperty(pRBM,0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = mxGetNumberOfElements(pBodies);

  const mxArray* pFrames = mxGetProperty(pRBM,0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = mxGetNumberOfElements(pFrames);

  // set up the model
  pm = mxGetField(featherstone,0,"NB");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.NB.  Are you passing in the correct structure?");
  model = new RigidBodyManipulator((int) mxGetScalar(pm), (int) mxGetScalar(pm), num_bodies, num_frames);

  pm = mxGetField(featherstone,0,"parent");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.parent.");
  double* parent = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"pitch");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.pitch.");
  double* pitch = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"dofnum");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.dofnum.");
  double* dofnum = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"damping");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.damping.");
  memcpy(model->damping.data(),mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"coulomb_friction");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_friction.");
  memcpy(model->coulomb_friction.data(),mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"static_friction");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.static_friction.");
  memcpy(model->static_friction.data(),mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"coulomb_window");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_window.");
  memcpy(model->coulomb_window.data(),mxGetPr(pm),sizeof(double)*model->NB);

  mxArray* pXtree = mxGetField(featherstone,0,"Xtree");
  if (!pXtree) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.Xtree.");

  mxArray* pI = mxGetField(featherstone,0,"I");
  if (!pI) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.I.");

  for (int i=0; i<model->NB; i++) {
    model->parent[i] = ((int) parent[i]) - 1;  // since it will be used as a C index
    model->pitch[i] = (int) pitch[i];
    model->dofnum[i] = (int) dofnum[i] - 1; // zero-indexed

    mxArray* pXtreei = mxGetCell(pXtree,i);
    if (!pXtreei) mexErrMsgIdAndTxt("Drake:HandCpmex:BadInputs","can't access model.featherstone.Xtree{%d}",i);

    // todo: check that the size is 6x6
    memcpy(model->Xtree[i].data(),mxGetPr(pXtreei),sizeof(double)*6*6);

    mxArray* pIi = mxGetCell(pI,i);
    if (!pIi) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't access model.featherstone.I{%d}",i);

    // todo: check that the size is 6x6
    memcpy(model->I[i].data(),mxGetPr(pIi),sizeof(double)*6*6);
  }

  for (int i=0; i<model->num_bodies; i++) {
    //DEBUG
    //cout << "constructModelmex: body " << i << endl;
    //END_DEBUG
    pm = mxGetProperty(pBodies,i,"linkname");
    mxGetString(pm,buf,100);
    model->bodies[i].linkname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"jointname");
    mxGetString(pm,buf,100);
//.........这里部分代码省略.........

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //mexPrintf("constructModelmex: START\n");
  //END_DEBUG
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  if (isa(prhs[0],"DrakeMexPointer")) {  // then it's calling the destructor
    destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]);
    return;
  }

  const mxArray* pRBM = prhs[0];
  RigidBodyManipulator *model=new RigidBodyManipulator();
  model->bodies.clear();  // a little gross:  the default constructor makes a body "world".  zap it because we will construct one again below

//  model->robot_name = get_strings(mxGetPropertySafe(pRBM,0,"name"));

  const mxArray* pBodies = mxGetPropertySafe(pRBM,0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies));

  const mxArray* pFrames = mxGetPropertySafe(pRBM,0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));

  for (int i = 0; i < num_bodies; i++) {
    //DEBUG
    //mexPrintf("constructModelmex: body %d\n",i);
    //END_DEBUG
    shared_ptr<RigidBody> b = make_shared<RigidBody>();
    b->body_index = i;

    b->linkname = mxGetStdString(mxGetPropertySafe(pBodies, i, "linkname"));

	  pm = mxGetPropertySafe(pBodies,i,"robotnum");
    b->robotnum = (int) mxGetScalar(pm)-1;

    pm = mxGetPropertySafe(pBodies,i,"mass");
    b->mass = mxGetScalar(pm);

    pm = mxGetPropertySafe(pBodies,i,"com");
    if (!mxIsEmpty(pm)) memcpy(b->com.data(),mxGetPrSafe(pm),sizeof(double)*3);

    pm = mxGetPropertySafe(pBodies,i,"I");
    if (!mxIsEmpty(pm)) memcpy(b->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6);

    pm = mxGetPropertySafe(pBodies,i,"position_num");
    b->position_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetPropertySafe(pBodies,i,"velocity_num");
    b->velocity_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetPropertySafe(pBodies,i,"parent");
    if (!pm || mxIsEmpty(pm))
      b->parent = nullptr;
    else {
      int parent_ind = static_cast<int>(mxGetScalar(pm))-1;
      if (parent_ind >= static_cast<int>(model->bodies.size()))
        mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size());
	  if (parent_ind >= 0)
		  b->parent = model->bodies[parent_ind];
    }

  	if (b->hasParent()) {
  	  string joint_name = mxGetStdString(mxGetPropertySafe(pBodies, i, "jointname"));
  	  //mexPrintf("adding joint %s\n", joint_name.c_str());

      pm = mxGetPropertySafe(pBodies, i, "Ttree");
      // todo: check that the size is 4x4
      Isometry3d transform_to_parent_body;
      memcpy(transform_to_parent_body.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4);

      int floating = (int) mxGetScalar(mxGetPropertySafe(pBodies, i, "floating"));

      Eigen::Vector3d joint_axis;
      pm = mxGetPropertySafe(pBodies, i, "joint_axis");
      memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);

      double pitch = mxGetScalar(mxGetPropertySafe(pBodies, i, "pitch"));

  	  std::unique_ptr<DrakeJoint> joint;
  	  switch (floating) {
    		case 0: {
    			if (pitch == 0.0) {
    				RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis);
    				joint = std::unique_ptr<RevoluteJoint>(revolute_joint);
    				setLimits(pBodies, i, revolute_joint);
    				setDynamics(pBodies, i, revolute_joint);
    			} else if (std::isinf(static_cast<double>(pitch))) {
    				PrismaticJoint *prismatic_joint = new PrismaticJoint(joint_name, transform_to_parent_body, joint_axis);
    				joint = std::unique_ptr<PrismaticJoint>(prismatic_joint);
    				setLimits(pBodies, i, prismatic_joint);
    				setDynamics(pBodies, i, prismatic_joint);
    			} else {
    				joint = std::unique_ptr<HelicalJoint>(new HelicalJoint(joint_name, transform_to_parent_body, joint_axis, pitch));
//.........这里部分代码省略.........