本文整理汇总了C++中Vector3d类的典型用法代码示例。如果您正苦于以下问题:C++ Vector3d类的具体用法?C++ Vector3d怎么用?C++ Vector3d使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Vector3d类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: latticeVectorHelper
Vector3d latticeVectorHelper( Vector3d difference_along_normal, Vector3d v_dir ){
double length = difference_along_normal.length() / (difference_along_normal.normalize().dot(v_dir));
return v_dir * length;
}
开发者ID:Pshriwise,项目名称:mcnp2cad,代码行数:4,代码来源:MCNPInput.cpp
示例2: position
/*Animate the camera in the timer event function
Note: La transformation de la camera est le seul parametre a etre animee.
Changer la position (ou le vecteur lateral ou up) ne fera rien lors de
l'animation.*/
void Widget3d::timerEvent( QTimerEvent* ipE )
{
if ( ipE->timerId() == mAnimationTimerId )
{
double animationTime = min( mAnimationDuration, mAnimationTimer.elapsed() );
double t = animationTime / (double)mAnimationDuration;
t = inversePower(t, 3);
/*Ici, on bircole une matrice (main droite) orthonormale qui réprésente
l'orientation de la caméra (pos, lat, up, look). On interpolera
la vielle orientation avec la nouvelle afin d'obtenir les positions,
ainsi que les vecteurs lat, up et look intermédiaires.*/
Vector3d lookVector( mOldCam.getLook(), mOldCam.getPos() );
lookVector.normalise();
Matrix4 m1( mOldCam.getLat(), mOldCam.getUp(), lookVector );
m1 = Matrix4( toVector(mOldCam.getPos()) ) * m1;
lookVector.set( mNewCam.getLook(), mNewCam.getPos() );
lookVector.normalise();
Matrix4 m2( mNewCam.getLat(), mNewCam.getUp(), lookVector );
m2 = Matrix4( toVector(mNewCam.getPos()) ) * m2;
Matrix4 iterationMatrix =
math::interpolate( m1, m2, t );
Vector3d interpolatedLook = toVector(mOldCam.getLook()) * (1 - t) +
toVector(mNewCam.getLook()) * t;
mCam.set( toPoint(iterationMatrix.getTranslationAsVector()),
toPoint(interpolatedLook),
Vector3d( iterationMatrix(1, 0), iterationMatrix(1, 1), iterationMatrix(1, 2) ) );
//--- animation de la projection
Camera::Projection iProj = mNewCam.getProjection();
Camera::Projection oldProj = mOldCam.getProjection();
Camera::Projection newProj = mNewCam.getProjection();
iProj.mRight = oldProj.mRight * (1-t) + newProj.mRight * t;
iProj.mLeft = oldProj.mLeft * (1-t) + newProj.mLeft * t;
iProj.mBottom = oldProj.mBottom * (1-t) + newProj.mBottom * t;
iProj.mTop = oldProj.mTop * (1-t) + newProj.mTop * t;
iProj.mNear = oldProj.mNear * (1-t) + newProj.mNear * t;
iProj.mFar = oldProj.mFar * (1-t) + newProj.mFar * t;
iProj.mZoomFactor = oldProj.mZoomFactor * (1-t) + newProj.mZoomFactor * t;
iProj.mType = newProj.mType;
iProj.mProportionalToWindow = newProj.mProportionalToWindow;
mCam.setProjection( iProj );
mCam.applyProjectionTransformation();
if ( animationTime >= mAnimationDuration )
{
killTimer( mAnimationTimerId );
mAnimationTimerId = 0;
}
update();
}
else if( ipE->timerId() == mCameraControlTimerId && !isAnimatingCamera() )
{
switch ( getControlType() )
{
case ctRotateAround:
{
Vector3d v; double a = 1.0 * PI / 180;
if( isKeyPressed( Qt::Key_W ) )
{ v += mCam.getLat(); a *= -1; }
if( isKeyPressed( Qt::Key_S ) )
{ v += mCam.getLat(); }
if( isKeyPressed( Qt::Key_A ) )
{ v += Vector3d(0, 1, 0); a *= -1; }
if( isKeyPressed( Qt::Key_D ) )
{ v += Vector3d(0, 1, 0); }
mCam.rotate( a, v, mCam.getLook() );
}break;
case ctFree:
{
Vector3d v;
if( isKeyPressed( Qt::Key_W ) )
{ v += Vector3d( mCam.getPos(), mCam.getLook() ); }
if( isKeyPressed( Qt::Key_S ) )
{ v += -Vector3d( mCam.getPos(), mCam.getLook() ); }
if( isKeyPressed( Qt::Key_A ) )
{ v += mCam.getLat() * -1; }
if( isKeyPressed( Qt::Key_D ) )
{ v += mCam.getLat(); }
if( isKeyPressed( Qt::Key_Q ) )
{ v += mCam.getUp(); }
if( isKeyPressed( Qt::Key_E ) )
{ v += mCam.getUp() * -1; }
v.normalise();
mCam.translate( v * getCameraSpeed() );
} break;
default: break;
}
update();
}
else
{
QWidget::timerEvent( ipE );
}
//.........这里部分代码省略.........
开发者ID:realisim,项目名称:realisim,代码行数:101,代码来源:Widget3d.cpp
示例3: setDirection
void ParticleState::setDirection(const Vector3d &dir) {
direction = dir / dir.getR();
}
开发者ID:phyytang,项目名称:CRPropa3,代码行数:3,代码来源:ParticleState.cpp
示例4: calcHeadingAngle
char* MyController::sendSceneInfo(std::string header, int camID) {
m_my->setWheelVelocity(0.0, 0.0);
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = calcHeadingAngle(); // y方向の回転は無しと考える
// カメラがついているリンク名取得
std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
if (camID < 3) {
link = m_my->getCameraLinkName(camID);
} else {
link = "WAIST_LINK0";
}
//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
Vector3d lpos;
m_my->getParts(link.c_str())->getPosition(lpos);
// カメラの位置取得(リンク座標系)
Vector3d cpos;
m_my->getCamPos(cpos, camID);
// カメラの位置(絶対座標系, ロボットの回転はないものとする)
//printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
//printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)),
lpos.y() + cpos.y(),
lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
//Vector3d campos(cpos.x(), cpos.y(), cpos.z());
// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
Vector3d cdir;
m_my->getCamDir(cdir, camID);
char *replyMsg = new char[1024];
sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
printf("%s \n", replyMsg);
m_srv->sendMsgToSrv(replyMsg);
return replyMsg;
}
开发者ID:bttung,项目名称:CleanUpRobotController,代码行数:45,代码来源:CleanUpRobot_1128.cpp
示例5: assert
void MomentumCorrFunc::correlateFrames(int frame1, int frame2) {
SimInfo::MoleculeIterator mi1;
SimInfo::MoleculeIterator mi2;
Molecule* mol1;
Molecule* mol2;
Molecule::AtomIterator ai1;
Molecule::AtomIterator ai2;
Atom* atom1;
Atom* atom2;
std::vector<Vector3d> atomPositions1;
std::vector<Vector3d> atomPositions2;
std::vector<Vector3d> atomVelocity1;
std::vector<Vector3d> atomVelocity2;
int thisAtom1, thisAtom2;
Snapshot* snapshot1 = bsMan_->getSnapshot(frame1);
Snapshot* snapshot2 = bsMan_->getSnapshot(frame2);
assert(snapshot1 && snapshot2);
RealType time1 = snapshot1->getTime();
RealType time2 = snapshot2->getTime();
int timeBin = int ((time2 - time1) /deltaTime_ + 0.5);
updateFrame(frame1);
atomPositions1.clear();
for (mol1 = info_->beginMolecule(mi1); mol1 != NULL;
mol1 = info_->nextMolecule(mi1)) {
for(atom1 = mol1->beginAtom(ai1); atom1 != NULL;
atom1 = mol1->nextAtom(ai1)) {
atomPositions1.push_back(atom1->getPos(frame1));
atomVelocity1.push_back(atom1->getVel(frame1));
}
}
updateFrame(frame2);
atomPositions2.clear();
for (mol2 = info_->beginMolecule(mi2); mol2 != NULL;
mol2 = info_->nextMolecule(mi2)) {
for(atom2 = mol2->beginAtom(ai2); atom2 != NULL;
atom2 = mol2->nextAtom(ai2)) {
atomPositions2.push_back(atom2->getPos(frame2));
atomVelocity2.push_back(atom2->getVel(frame2));
}
}
thisAtom1 = 0;
for (mol1 = info_->beginMolecule(mi1); mol1 != NULL;
mol1 = info_->nextMolecule(mi1)) {
for(atom1 = mol1->beginAtom(ai1); atom1 != NULL;
atom1 = mol1->nextAtom(ai1)) {
Vector3d r1 = atomPositions1[thisAtom1];
Vector3d p1 = atom1->getMass() * atomVelocity1[thisAtom1];
thisAtom2 = 0;
for (mol2 = info_->beginMolecule(mi2); mol2 != NULL;
mol2 = info_->nextMolecule(mi2)) {
for(atom2 = mol2->beginAtom(ai2); atom2 != NULL;
atom2 = mol2->nextAtom(ai2)) {
Vector3d r2 = atomPositions2[thisAtom2];
Vector3d p2 = atom2->getMass() * atomVelocity1[thisAtom1];
Vector3d deltaPos = (r2-r1);
Vector3d dp2( deltaPos.x() * deltaPos.x(),
deltaPos.y() * deltaPos.y(),
deltaPos.z() * deltaPos.z());
Vector3d pprod( p1.x() * p2.x(),
p1.y() * p2.y(),
p1.z() * p2.z());
histogram_[timeBin] += outProduct(dp2, pprod);
thisAtom2++;
}
}
thisAtom1++;
}
}
count_[timeBin]++;
}
开发者ID:Patrick-Louden,项目名称:2.2,代码行数:88,代码来源:MomentumCorrFunc.cpp
示例6: vectorAngle
double vectorAngle(const Vector3d &r1,const Vector3d &r2){
return(acos(r1.dot(r2)/r1.norm()/r2.norm()));
}
开发者ID:jyh1,项目名称:Homework-CompPhyC2,代码行数:3,代码来源:zmatrix.cpp
示例7: Inside
bool ContainedByBox::Inside(const Vector3d& point) const
{
return ((point.x() >= corner1.x()) && (point.x() <= corner2.x()) &&
(point.y() >= corner1.y()) && (point.y() <= corner2.y()) &&
(point.z() >= corner1.z()) && (point.z() <= corner2.z()));
}
开发者ID:arjangcore,项目名称:povray,代码行数:6,代码来源:object.cpp
示例8: idle
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
optotrak->updateMarkers();
//cerr << deltaT << endl;
markers = optotrak->getAllMarkers();
// Coordinates picker
allVisiblePlatform = isVisible(markers[1].p);
allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p);
allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p);
allVisibleHead = allVisiblePatch && isVisible(markers[1].p);
if ( allVisiblePatch )
headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p);
if ( allVisibleThumb )
thumbCoords.update(markers[15].p,markers[17].p,markers[18].p);
if ( allVisibleIndex )
indexCoords.update(markers[13].p, markers[14].p, markers[16].p );
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{
if ( !allVisibleIndex )
occludedFrames++;
if ( !allVisibleThumb )
occludedFrames++;
}
if(headCalibration)
{
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
} else {
eyeRight = Vector3d(interoculardistance/2,0,0);
eyeLeft = -eyeRight;
}
thumb = thumbCoords.getP1();
index = indexCoords.getP1();
singleMarker = markers.at(4).p.transpose();
if( abs(index.z()-thumb.z()) > 15.0 )
fingersAtStart=false;
else
fingersAtStart=true;
if( endTrial && fingersAtStart)
{
advanceTrial();
}
#ifdef WRITE
// Write to file
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{
markersFile << fixed << trialNumber << "\t" <<
eyeLeft.transpose() << "\t" << eyeRight.transpose() << "\t" <<
//markers.at(4).p.transpose() << "\t" <<
index.transpose() << "\t" <<
thumb.transpose() << "\t" <<
fingersAtStart << "\t" <<
isStimulusDrawn
;
markersFile << endl;
}
#endif
}
开发者ID:guendas,项目名称:cncsvision,代码行数:76,代码来源:ExpOct2012grasp4.0.cpp
示例9: b
void Gelatin::step(double h, const Vector3d &grav, const vector< shared_ptr<FreakFace> > faces)
{
aTrips.clear();
//v.setZero();
//f.setZero();
//
// vTrips.clear();
// fTrips.clear();
VectorXd b(n);
b.setZero();
for (int i = 0; i < particles.size(); i++) {
if (!particles[i]->fixed) {
int index = particles[i]->i;
double m = particles[i]->m;
double val = m + h * damping(0) * m;
aTrips.push_back(Trip(index, index, val));
aTrips.push_back(Trip(index+1, index+1, val));
aTrips.push_back(Trip(index+2, index+2, val));
Vector3d v = particles[i]->v;
Vector3d fg = h * m * grav;
b(index) = m * v(0) + fg(0);
b(index+1) = m * v(1) + fg(1);
b(index+2) = m * v(2) + fg(2);
}
}
for (int i = 0; i < springs.size(); i++) {
double E = springs[i]->E;
double L = springs[i]->L;
auto p0 = springs[i]->p0;
auto p1 = springs[i]->p1;
//MatrixXd delta(3, 1);
Vector3d delta = p1->x - p0->x;
double l = delta.norm();
Vector3d fs = E * (l - L) * (delta / l);
if (!springs[i]->p0->fixed) {
b(p0->i) += h * fs(0);
b(p0->i+1) += h * fs(1);
b(p0->i+2) += h * fs(2);
}
if (!springs[i]->p1->fixed) {
b(p1->i) -= h * fs(0);
b(p1->i+1) -= h * fs(1);
b(p1->i+2) -= h * fs(2);
}
if (!springs[i]->p0->fixed && !springs[i]->p1->fixed) {
Matrix3d ks = (E / (l * l)) * ((1.0 - (l - L)/l) * (delta * delta.transpose())
+ ((l - L)/l) * double(delta.transpose() * delta) * Matrix3d::Identity());
int i0 = p0->i;
int i1 = p1->i;
addKs(ks, i0, i1, h);
}
}
Eigen::SparseMatrix<double> sparseA(n,n);
sparseA.setFromTriplets(aTrips.begin(), aTrips.end());
ConjugateGradient< SparseMatrix<double> > cg;
cg.setMaxIterations(25);
cg.setTolerance(1e-3);
cg.compute(sparseA);
//v = cg.solve(b);
v = cg.solveWithGuess(b, v);
//v = A.ldlt().solve(b);
for (int i = 0; i < particles.size(); i++) {
if (!particles[i]->fixed) {
int index = particles[i]->i;
particles[i]->v = v.block<3,1>(index, 0);
particles[i]->x += h * particles[i]->v;
}
}
collide(faces);
// Update position and normal buffers
updatePosNor();
}
开发者ID:petegodkin,项目名称:AnimationFinalProj,代码行数:89,代码来源:Gelatin.cpp
示例10: ot_index_box
void ot_index_box(const Vector3d& min_point, const Vector3d& max_point, OT_ID *id)
{
// TODO OPTIMIZE
DBL dx, dy, dz, desiredSize;
DBL bsized, maxord;
POW2OP_DECLARE()
// Calculate the absolute minimum required size of the node, assuming it is perfectly centered within the node;
// Node size must be a power of 2, and be large enough to accomodate box's biggest dimensions with maximum overhang to all sides
// compute ideal size of the node for a perfect fit without any overhang
dx = max_point.x() - min_point.x();
dy = max_point.y() - min_point.y();
dz = max_point.z() - min_point.z();
desiredSize = max3(dx, dy, dz);
// compute ideal size of the node for a perfect fit with full overhang to all sides
// desiredSize /= (1 + 2 * 0.5);
// compute best-matching power-of-two size for a perfect fit with overhang
// (Note: theoretically this might pick a size larger than required if desiredSize is already a power of two)
// desiredSize *= 2.0;
POW2OP_FLOOR(bsized,desiredSize)
// avoid divisions by zero
if(bsized == 0.0)
bsized = 1.0;
#ifdef SAFE_METHOD
// This block checks for the case where the node id would cause integer
// overflow, since it is a small buffer far away
maxord = max3(fabs(min_point[X]), fabs(min_point[Y]), fabs(min_point[Z]));
maxord += OT_BIAS;
while (maxord / bsized > 1000000000.0)
{
#ifdef RADSTATS
overflows++;
#endif
bsized *= 2.0;
}
#endif // SAFE_METHOD
// The node we chose so far would be ideal for a box of identical size positioned at the node's center,
// but the actual box is probably somewhat off-center and therefore may have excessive overhang in some directions;
// check and possibly fix this.
Vector3d center = (min_point + max_point) / 2;
id->x = (int) floor((center[X] + OT_BIAS) / bsized);
id->y = (int) floor((center[Y] + OT_BIAS) / bsized);
id->z = (int) floor((center[Z] + OT_BIAS) / bsized);
POW2OP_ENCODE(id->Size, bsized)
#ifdef RADSTATS
thisloops = 0;
#endif
while (!ot_point_in_node(min_point, id) || !ot_point_in_node(max_point, id))
{
// Debug_Info("looping %d,%d,%d,%d min=%d, max=%d\n", test_id.x, test_id.y,
// test_id.z, test_id.Size, ot_point_in_node(min_point, &test_id),
// ot_point_in_node(max_point, &test_id));
ot_parent(id, id);
#ifdef RADSTATS
totloops++;
thisloops++;
#endif
}
#ifdef RADSTATS
if (thisloops < minloops)
minloops = thisloops;
if (thisloops > maxloops)
maxloops = thisloops;
#endif
#ifdef OT_DEBUG
if (id->Size > 139)
{
Debug_Info("unusually large id, maxdel=%.4f, bsized=%.4f, isize=%d\n",
maxdel, bsized, id->Size);
}
#endif
}
开发者ID:Clodo76,项目名称:povray,代码行数:83,代码来源:octree.cpp
示例11: u
void MotionModelScanMatcher::estimateCovariance(RobotPose max_pose, PoseGrid& grid, int use_fine_grid){
int posegrid_x_dim = pose_x_dim;
int posegrid_y_dim = pose_y_dim;
if(use_fine_grid) posegrid_x_dim = fine_pose_x_dim;
if(use_fine_grid) posegrid_y_dim = fine_pose_y_dim;
Matrix3d cov_temp;
Matrix3d K = Matrix3d::Constant(0.0);
Vector3d u(0.0, 0.0, 0.0);
// first normalize grid
double sum = 0;
double offset = grid.getVal(max_pose_ind.theta, max_pose_ind.y, max_pose_ind.x) - 25;
double phat;
double logp;
RobotPose p, norm_p;
for (int theta_ind = 0; theta_ind < pose_theta_dim; theta_ind++) {
p.theta = theta_ind*theta_step;
for (int x_ind = 0; x_ind < posegrid_x_dim; x_ind++) {
p.x = (x_ind + pose_grid_offset_x - occ_x_dim/2)*x_step;
for (int y_ind = 0; y_ind < posegrid_y_dim; y_ind++) {
p.y = (y_ind + pose_grid_offset_y - occ_x_dim/2)*y_step;
//p is now in the same frame as max_pose
logp = grid.getVal(theta_ind, y_ind, x_ind);
if(logp == -DBL_MAX){
continue;
}
if(sum <= -DBL_MAX/2 || sum > DBL_MAX/2){
ROS_WARN("[MotionModelScanMatcher::estimateCovariance] - SUM NEAR OVERFLOW!!!!!!!!!!!");
}
//re-scale the log prob to an actual prob
//ROS_INFO("[MotionModelScanMatcher::estimateCovariance]\tlogp=%f",logp);
phat = exp(logp - offset); // legit fudge but overflows still
//phat = exp((logp / 10) + offset); // fudge
//calc running sums
Vector3d pose = p.toEigenVector();
Matrix3d plus_k = phat * (pose * pose.transpose());
K = K + plus_k;
u = u + (pose * phat);
sum += phat;
/*
if(phat != 0){
ROS_INFO("[MotionModelScanMatcher::estimateCovariance] - phat = %f,\t logp = %f, offset = %f", phat, logp, offset);
ROS_INFO("[MotionModelScanMatcher::estimateCovariance] - u = [%f %f %f]", u(0,0), u(1,0), u(2,0));
}
*/
}
}
}
cov_temp = K / sum + (u * u.transpose())/(pow(sum,2));
//move cov_temp's values to cov
for(int i = 0; i < 9; i ++){
int x_ind = i % 3;
int y_ind = i / 3;
cov[i] = cov_temp(x_ind, y_ind);
}
ROS_INFO("[MotionModelScanMatcher::estimateCovariance]\tSigma=[%e\t%e\t%e]",cov[0],cov[1],cov[2]);
ROS_INFO(" \t [%e\t%e\t%e]",cov[3],cov[4],cov[5]);
ROS_INFO(" \t [%e\t%e\t%e]",cov[6],cov[7],cov[8]);
}
开发者ID:embr,项目名称:cs225b,代码行数:70,代码来源:MotionModelScanMatcher.cpp
示例12: mexFunction
//.........这里部分代码省略.........
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
int nc = mxGetNumberOfElements(mxBodyContactPts);
if (nc<1) continue;
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
pr = mxGetPr(mxBodyContactPts);
for (j=0; j<nc; j++) {
se.contact_pt_inds.insert((int)pr[j]-1);
}
se.contact_surface = (int) pContactSurfaces[i]-1;
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
pdata->r->HandC(q,qd,(MatrixXd*)NULL,pdata->H,pdata->C,(MatrixXd*)NULL,(MatrixXd*)NULL,(MatrixXd*)NULL);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
Map<VectorXd> qdvec(qd,nq);
MatrixXd B,JB,Jp,Jpdot,normals;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,terrain_height);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qdvec;
开发者ID:RussTedrake,项目名称:drake,代码行数:67,代码来源:QPControllermex.cpp
示例13: main
int main( int /*argc*/, char* argv[] )
{
const static string ui_file = "app.clicks";
// Load configuration data
pangolin::ParseVarsFile("app.cfg");
InputRecordRepeat input("ui.");
input.LoadBuffer(ui_file);
// Target to track from
Target target;
target.GenerateRandom(60,25/(842.0/297.0),75/(842.0/297.0),15/(842.0/297.0),Vector2d(297,210));
// target.GenerateCircular(60,20,50,15,makeVector(210,210));
// target.GenerateEmptyCircle(60,25,75,15,200,makeVector(297,210));
target.SaveEPS("target_A4.eps");
cout << "Calibration target saved as: target_A4.eps" << endl;
// Setup Video
Var<string> video_uri("video_uri");
VideoRecordRepeat video(video_uri, "video.pvn", 1024*1024*200);
// VideoInput video(video_uri);
const unsigned w = video.Width();
const unsigned h = video.Height();
// Create Glut window
pangolin::CreateGlutWindowAndBind("Main",2*w+PANEL_WIDTH,h);
// Pangolin 3D Render state
pangolin::OpenGlRenderState s_cam;
s_cam.Set(ProjectionMatrixRDF_TopLeft(640,480,420,420,320,240,1,1E6));
s_cam.Set(FromTooN(toTooN(Sophus::SE3(Sophus::SO3(),Vector3d(-target.Size()[0]/2,-target.Size()[1]/2,500) ))));
pangolin::Handler3D handler(s_cam);
// Create viewport for video with fixed aspect
View& vPanel = pangolin::CreatePanel("ui").SetBounds(1.0,0.0,0,PANEL_WIDTH);
View& vVideo = pangolin::Display("Video").SetAspect((float)w/h);
View& v3D = pangolin::Display("3D").SetAspect((float)w/h).SetHandler(&handler);
// View& vDebug = pangolin::Display("Debug").SetAspect((float)w/h);
Display("Container")
.SetBounds(1.0,0.0,PANEL_WIDTH,1.0,false)
.SetLayout(LayoutEqual)
.AddDisplay(vVideo)
.AddDisplay(v3D)
// .AddDisplay(vDebug)
;
// OpenGl Texture for video frame
GlTexture texRGB(w,h,GL_RGBA8);
GlTexture tex(w,h,GL_LUMINANCE8);
// Declare Image buffers
CVD::ImageRef size(w,h);
Image<Rgb<byte> > Irgb(size);
Image<byte> I(size);
Image<float> intI(size);
Image<short> lI(size);
Image<std::array<float,2> > dI(size);
Image<byte> tI(size);
// Camera parameters
Matrix<float,5,1> cam_params; // = Var<Matrix<float,5,1> >("cam_params");
cerr << "Not loading params properly" << endl;
cam_params << 0.694621, 0.925258, 0.505055, 0.484551, 0.968455;
FovCamera cam( w,h, w*cam_params[0],h*cam_params[1], w*cam_params[2],h*cam_params[3], cam_params[4] );
// Last good pose
Sophus::SE3 T_gw;
std::clock_t last_good;
int good_frames;
// Pose hypothesis
Sophus::SE3 T_hw;
// Fixed mirrored pose
Sophus::SE3 T_0w;
// Stored keyframes
boost::ptr_vector<Keyframe> keyframes;
// Variables
Var<bool> record("ui.Record",false,false);
Var<bool> play("ui.Play",false,false);
Var<bool> source("ui.Source",false,false);
Var<bool> add_keyframe("ui.Add Keyframe",false,false);
Var<bool> use_mirror("ui.Use Mirror",false,true);
Var<bool> draw_mirror("ui.Draw Mirror",false,true);
// Var<bool> calc_mirror_pose("ui.Calculate Mirrored Pose",false,false);
Var<bool> disp_thresh("ui.Display Thresh",false);
Var<float> at_threshold("ui.Adap Threshold",1.0,0,1.0);
Var<int> at_window("ui.Adapt Window",w/3,1,200);
Var<float> conic_min_area(".Conic min area",25, 0, 100);
Var<float> conic_max_area(".Conic max area",4E4, 0, 1E5);
Var<float> conic_min_density(".Conic min density",0.4, 0, 1.0);
Var<float> conic_min_aspect(".Conic min aspect",0.1, 0, 1.0);
//.........这里部分代码省略.........
开发者ID:obiou,项目名称:fiducials,代码行数:101,代码来源:main.cpp
示例14: addObservation
void MapInitializer::addObservation(Vector3d & X, Vector2d pt, Transformation<double> & pose,
const ICamera * cam, const Transformation<double> & TbaseCam)
{
CostFunction * costFunc = new ReprojectionErrorStereo(pt, TbaseCam, cam);
problem.AddResidualBlock(costFunc, NULL, X.data(), pose.transData(), pose.rotData());
}
开发者ID:BKhomutenko,项目名称:spcslam,代码行数:6,代码来源:cartography.cpp
示例15: reader
void BondAngleDistribution::process() {
Molecule* mol;
Atom* atom;
RigidBody* rb;
int myIndex;
SimInfo::MoleculeIterator mi;
Molecule::RigidBodyIterator rbIter;
Molecule::AtomIterator ai;
StuntDouble* sd;
Vector3d vec;
std::vector<Vector3d> bondvec;
RealType r;
int nBonds;
int i;
DumpReader reader(info_, dumpFilename_);
int nFrames = reader.getNFrames();
frameCounter_ = 0;
nTotBonds_ = 0;
for (int istep = 0; istep < nFrames; istep += step_) {
reader.readFrame(istep);
frameCounter_++;
currentSnapshot_ = info_->getSnapshotManager()->getCurrentSnapshot();
if (evaluator_.isDynamic()) {
seleMan_.setSelectionSet(evaluator_.evaluate());
}
// update the positions of atoms which belong to the rigidbodies
for (mol = info_->beginMolecule(mi); mol != NULL;
mol = info_->nextMolecule(mi)) {
for (rb = mol->beginRigidBody(rbIter); rb != NULL;
rb = mol->nextRigidBody(rbIter)) {
rb->updateAtoms();
}
}
// outer loop is over the selected StuntDoubles:
for (sd = seleMan_.beginSelected(i); sd != NULL;
sd = seleMan_.nextSelected(i)) {
myIndex = sd->getGlobalIndex();
nBonds = 0;
bondvec.clear();
// inner loop is over all other atoms in the system:
for (mol = info_->beginMolecule(mi); mol != NULL;
mol = info_->nextMolecule(mi)) {
for (atom = mol->beginAtom(ai); atom != NULL;
atom = mol->nextAtom(ai)) {
if (atom->getGlobalIndex() != myIndex) {
vec = sd->getPos() - atom->getPos();
if (usePeriodicBoundaryConditions_)
currentSnapshot_->wrapVector(vec);
// Calculate "bonds" and make a pair list
r = vec.length();
// Check to see if neighbor is in bond cutoff
if (r < rCut_) {
// Add neighbor to bond list's
bondvec.push_back(vec);
nBonds++;
nTotBonds_++;
}
}
}
for (int i = 0; i < nBonds-1; i++ ){
Vector3d vec1 = bondvec[i];
vec1.normalize();
for(int j = i+1; j < nBonds; j++){
Vector3d vec2 = bondvec[j];
vec2.normalize();
RealType theta = acos(dot(vec1,vec2))*180.0/NumericConstant::PI;
if (theta > 180.0){
theta = 360.0 - theta;
}
int whichBin = int(theta/deltaTheta_);
histogram_[whichBin] += 2;
}
}
}
}
//.........这里部分代码省略.........
开发者ID:Patrick-Louden,项目名称:2.2,代码行数:101,代码来源:BondAngleDistribution.cpp
示例16: startPoint
// Convert to Printlines
void Layer::MakePrintlines(Vector3d &lastPos, //GCodeState &state,
vector<PLine3> &lines3,
double offsetZ,
Settings &settings) const
{
const double linewidth = settings.GetExtrudedMaterialWidth(thickness);
const double cornerradius = linewidth*settings.get_double("Slicing","CornerRadius");
const bool clipnearest = settings.get_boolean("Slicing","MoveNearest");
const uint supportExtruder = settings.GetSupportExtruder();
const double minshelltime = settings.get_double("Slicing","MinShelltime");
const double maxshellspeed = settings.get_double("Extruder","MaxShellSpeed");
const bool ZliftAlways = settings.get_boolean("Extruder","ZliftAlways");
Vector2d startPoint(lastPos.x(),lastPos.y());
const double extr_per_mm = settings.GetExtrusionPerMM(thickness);
//vector<PLine3> lines3;
Printlines printlines(this, &settings, offsetZ);
vector<PLine2> lines;
vector<Poly> polys; // intermediate collection
// polys to keep line movements inside
//const vector<Poly> * clippolys = &polygons;
const vector<Poly> * clippolys = GetOuterShell();
// 1. Skins, all but last, because they are the lowest lines, below layer Z
if (skins > 1) {
for(uint s = 0; s < skins; s++) {
// z offset from bottom to top:
double skin_z = Z - thickness + (s+1)*thickness/skins;
if ( skin_z < 0 ){
cerr << "Skin Z<0! " << s << " -- " << Z << " -- "<<skin_z <<" -- " << thickness << endl;
continue;
}
// skin infill polys:
if (skinFullInfills[s])
polys.insert(polys.end(),
skinFullInfills[s]->infillpolys.begin(),
skinFullInfills[s]->infillpolys.end());
// add skin infill to lines
printlines.addPolys(INFILL, polys, false);
polys.clear();
// make polygons at skin_z:
for(size_t p = 0; p < skinPolygons.size(); p++) {
polys.push_back(Poly(skinPolygons[p], skin_z));
}
// add skin to lines
printlines.addPolys(SKIN, polys, (s==0), // displace at first skin
maxshellspeed * 60,
minshelltime);
if (s < skins-1) { // not on the last layer, this handle with all other lines
// have to get all these separately because z changes
printlines.makeLines(startPoint, lines);
if (!ZliftAlways)
printlines.clipMovements(*clippolys, lines, clipnearest, linewidth);
printlines.optimize(linewidth,
minshelltime, cornerradius, lines);
printlines.getLines(lines, lines3, extr_per_mm);
printlines.clear();
lines.clear();
}
polys.clear();
}
} // last skin layer now still in lines
lines.clear();
// 2. Skirt
printlines.addPolys(SKIRT, skirtPolygons, false,
maxshellspeed * 60,
minshelltime);
// 3. Support
if (supportInfill) {
uint extruderbefore = settings.selectedExtruder;
settings.SelectExtruder(supportExtruder);
printlines.addPolys(SUPPORT, supportInfill->infillpolys, false);
settings.SelectExtruder(extruderbefore);
}
// 4. all other polygons:
// Shells
for(int p=shellPolygons.size()-1; p>=0; p--) { // inner to outer
printlines.addPolys(SHELL, shellPolygons[p],
(p==(int)(shellPolygons.size())-1),
maxshellspeed * 60,
minshelltime);
}
// Infill
if (normalInfill)
//.........这里部分代码省略.........
开发者ID:keesj,项目名称:repsnapper,代码行数:101,代码来源:layer.cpp
示例17: se2bracket_tests
bool se2bracket_tests()
{
bool failed = false;
vector<Vector3d> vecs;
Vector3d tmp;
// tmp << 0,0,0;
// vecs.push_back(tmp);
// tmp << 1,0,0;
// vecs.push_back(tmp);
// tmp << 0,1,1;
// vecs.push_back(tmp);
// tmp << -1,1,0;
// vecs.push_back(tmp);
tmp << 20,-1,-1;
vecs.push_back(tmp);
// tmp << 30,5,20;
// vecs.push_back(tmp);
for (unsigned int i=0; i<vecs.size(); ++i)
{
Vector3d resDiff = vecs[i] - SE2::vee(SE2::hat(vecs[i]));
if (resDiff.norm()>SMALL_EPS)
{
cerr << "Hat-vee Test" << endl;
cerr << "Test case: " << i << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
}
for (unsigned int j=0; j<vecs.size(); ++j)
{
Vector3d res1 = SE2::lieBracket(vecs[i],vecs[j]);
Matrix3d hati = SE2::hat(vecs[i]);
Matrix3d hatj = SE2::hat(vecs[j]);
Vector3d res2 = SE2::vee(hati*hatj-hatj*hati);
Vector3d resDiff = res1-res2;
if (resDiff.norm()>SMALL_EPS)
{
cerr << "SE2 Lie Bracket Test" << endl;
cerr << "Test case: " << i << ", " <<j<< endl;
cerr << vecs[i].transpose() << endl;
cerr << vecs[j].transpose() << endl;
cerr << res1 << endl;
cerr << res2 << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
failed = true;
}
}
Vector3d omega = vecs[i];
Matrix3d exp_x = SE2::exp(omega).matrix();
Matrix3d expmap_hat_x = (SE2::hat(omega)).exp();
Matrix3d DiffR = exp_x-expmap_hat_x;
double nrm = DiffR.norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "expmap(hat(x)) - exp(x)" << endl;
cerr << "Test case: " << i << endl;
cerr << exp_x <<endl;
cerr << expmap_hat_x <<endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
}
return failed;
}
开发者ID:jbohren-forks,项目名称:sophus,代码行数:72,代码来源:test_se2.cpp
示例18: direction
GLThread::GLThread() {
int numInit = (NUM_POINTS-3)/2;
double noise_factor = 0.0;
display_start_pos.setZero();
vector<Vector3d> vertices;
vector<double> angles;
vertices.push_back(Vector3d::Zero());
angles.push_back(0.0);
//push back unitx so first tangent matches start_frame
vertices.push_back(Vector3d::UnitX()*DEFAULT_REST_LENGTH);
angles.push_back(0.0);
Vector3d direction;
direction(0) = 1.0;
direction(1) = 0.0;
direction(2) = -1.0;
direction.normalize();
for (int i=0; i < numInit; i++)
{
Vector3d noise( ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0);
noise *= noise_factor;
Vector3d next_Vec = vertices.back()+(direction+noise).normalized()*DEFAULT_REST_LENGTH;
vertices.push_back(next_Vec);
angles.push_back(0.0);
//std::cout << positions[i] << std::endl << std::endl;
}
//change direction
direction(0) = 1.0;
direction(1) = 0.0;
direction(2) = 1.0;
direction.normalize();
for (int i=0; i < numInit; i++)
{
Vector3d noise( ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0);
noise *= noise_factor;
Vector3d next_Vec = vertices.back()+(direction+noise).normalized()*DEFAULT_REST_LENGTH;
vertices.push_back(next_Vec);
angles.push_back(0.0);
}
//push back unitx so last tangent matches end_frame
vertices.push_back(vertices.back()+Vector3d::UnitX()*DEFAULT_REST_LENGTH);
angles.push_back(0.0);
angles.resize(vertices.size());
rotations[0] = Matrix3d::Identity();
rotations[1] = Matrix3d::Identity();
_thread = new Thread(vertices, angles, rotations[0], rotations[1]);
//_thread->set_start_rest_length(1);
//_thread->set_end_rest_length(1);
//_thread->set_rest_length(DEFAULT_REST_LENGTH);
#ifdef ISOTROPIC
to_set_bend = _thread->get_bend_coeff();
#else
to_set_B = _thread->get_bend_matrix();
#endif
to_set_twist = _thread->get_twist_coeff();
to_set_grav = _thread->get_grav_coeff();
#ifdef ISOTROPIC
_thread->set_coeffs_normalized(to_set_bend, to_set_twist, to_set_grav);
#else
_thread->set_coeffs_normalized(to_set_B, to_set_twist, to_set_grav);
#endif
InitContour();
strcpy(_display_name, "");
}
开发者ID:bo-wu,项目名称:surgical,代码行数:83,代码来源:glThread.cpp
示例19: idle
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
optotrak.updateMarkers();
markers = optotrak.g
|
请发表评论