本文整理汇总了C++中Vector4d::data方法的典型用法代码示例。如果您正苦于以下问题：C++ Vector4d::data方法的具体用法？C++ Vector4d::data怎么用？C++ Vector4d::data使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector4d的用法示例。
在下文中一共展示了Vector4d::data方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: rigidBodyConstraintParseQuat
void rigidBodyConstraintParseQuat(const mxArray* pm, Vector4d &quat)
{
if(!mxIsNumeric(pm))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be a 4x1 double vector");
}
if(!(mxGetM(pm) == 4 && mxGetN(pm) == 1))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be of size 4x1");
}
memcpy(quat.data(),mxGetPr(pm),sizeof(double)*4);
for(int i = 0;i<4;i++)
{
if((mxIsInf(quat(i)))||(mxIsNaN(quat(i))))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 cannot have entry equal to NaN or Inf");
}
}
double quat_norm = quat.norm();
if(quat_norm==0.0)
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The Input argument 1 must be a nonzero vector");
}
quat = quat/quat_norm;
}示例2: mexFunction
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
if(nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:testQuatmex:BadInputs","Usage [r,dr,e,ed,quat,dquat,q3,dq3,w,dw] = testQuatmex(q1,q2,axis,u,v)");
}
Vector4d q1;
Vector4d q2;
memcpy(q1.data(),mxGetPr(prhs[0]),sizeof(double)*4);
memcpy(q2.data(),mxGetPr(prhs[1]),sizeof(double)*4);
Vector4d r = quatDiff(q1,q2);
Matrix<double,4,8> dr = dquatDiff(q1,q2);
plhs[0] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[0]),r.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[1]),dr.data(),sizeof(double)*4*8);
Vector3d axis;
memcpy(axis.data(),mxGetPr(prhs[2]),sizeof(double)*3);
double e = quatDiffAxisInvar(q1,q2,axis);
Matrix<double,1,11> de = dquatDiffAxisInvar(q1,q2,axis);
plhs[2] = mxCreateDoubleScalar(e);
plhs[3] = mxCreateDoubleMatrix(1,11,mxREAL);
memcpy(mxGetPr(plhs[3]),de.data(),sizeof(double)*11);
Vector3d u,v;
Vector4d quat;
Matrix<double,4,6> dquat;
memcpy(u.data(),mxGetPr(prhs[3]),sizeof(double)*3);
memcpy(v.data(),mxGetPr(prhs[4]),sizeof(double)*3);
Vector4d q3 = quatProduct(q1,q2);
Matrix<double,4,8> dq3 = dquatProduct(q1,q2);
plhs[4] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[5] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[4]),q3.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[5]),dq3.data(),sizeof(double)*4*8);
Vector3d w = quatRotateVec(q1,u);
Matrix<double,3,7> dw = dquatRotateVec(q1,u);
plhs[6] = mxCreateDoubleMatrix(3,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(3,7,mxREAL);
memcpy(mxGetPr(plhs[6]),w.data(),sizeof(double)*3);
memcpy(mxGetPr(plhs[7]),dw.data(),sizeof(double)*3*7);
}示例3: vertex
inline void vertex(const Vector4d& v)
{
#if USE_VERTEX_BUFFER
data[currentPosition++] = v.cast<float>();
++currentStripLength;
if (currentPosition == capacity)
{
flush();
data[0] = v.cast<float>();
currentPosition = 1;
currentStripLength = 1;
}
#else
glVertex3dv(v.data());
#endif
}示例4: mexFunction
//.........这里部分代码省略.........
if(nrhs<=7)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[7],tspan);
}
if(!mxIsNumeric(prhs[2]) || !mxIsNumeric(prhs[3]) || !mxIsNumeric(prhs[4]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","iAfun, jAvar and A must be numeric");
}
int lenA = mxGetM(prhs[2]);
if(mxGetM(prhs[3]) != lenA || mxGetM(prhs[4]) != lenA || mxGetN(prhs[2]) != 1 || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","iAfun,jAvar,A must be column vectors of the same size");
}
VectorXi iAfun(lenA);
VectorXi jAvar(lenA);
VectorXd A(lenA);
for(int i = 0;i<lenA;i++)
{
iAfun(i) = (int) *(mxGetPr(prhs[2])+i)-1;
jAvar(i) = (int) *(mxGetPr(prhs[3])+i)-1;
A(i) = *(mxGetPr(prhs[4])+i);
}
if(!mxIsNumeric(prhs[5]) || !mxIsNumeric(prhs[6]) || mxGetM(prhs[5]) != mxGetM(prhs[6]) || mxGetN(prhs[5]) != 1 || mxGetN(prhs[6]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub must be numeric column vectors of the same size");
}
int num_constraint = mxGetM(prhs[5]);
VectorXd lb(num_constraint);
VectorXd ub(num_constraint);
memcpy(lb.data(),mxGetPr(prhs[5]),sizeof(double)*num_constraint);
memcpy(ub.data(),mxGetPr(prhs[6]),sizeof(double)*num_constraint);
SingleTimeLinearPostureConstraint* cnst = new SingleTimeLinearPostureConstraint(model,iAfun,jAvar,A,lb,ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","SingleTimeLinearPostureConstraint");
}
break;
// AllBodiesClosestDistanceConstraint
case RigidBodyConstraint::AllBodiesClosestDistanceConstraintType:
{
//DEBUG
//cout << "nrhs = " << nrhs << endl;
//END_DEBUG
if(nrhs != 5 && nrhs != 6)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs",
"Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::AllBodiesClosestDistanceConstraintType, robot.mex_model_ptr,lb,ub,active_collision_options,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs == 5)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[5],tspan);
}
double lb = (double) mxGetScalar(prhs[2]);
double ub = (double) mxGetScalar(prhs[3]);
// Parse `active_collision_options`
vector<int> active_bodies_idx;