// spin, setting the state to different values void MyInfo::setThreadFunc( robot_interaction::LockedRobotState* locked_state, int* counter, double offset) { bool go = true; while(go) { double val = offset; for (int loops = 0 ; loops < 100 ; ++loops) { val += 0.0001; robot_state::RobotState cp1(*locked_state->getState()); cp1.setVariablePosition(JOINT_A, val + 0.00001); cp1.setVariablePosition(JOINT_C, val + 0.00002); cp1.setVariablePosition(JOINT_F, val + 0.00003); locked_state->setState(cp1); } cnt_lock_.lock(); go = !quit_; ++*counter; cnt_lock_.unlock(); checkState(*locked_state); val += 0.000001; } }
int main(int argc, char const *argv[]) { int n; printf("复制文件还是目录? 文件输入:1 目录输入:2\n"); scanf("%d",&n); if(n==1) { char filename1[128]; char filename2[128]; printf("请输入你想复制的文件名!\n"); scanf("%s",&filename1); printf("请输入你复制后的文件名!\n"); scanf("%s",&filename2); cp1(filename1,filename2); } else if(n==2) { char filename1[128]; char filename2[128]; printf("请输入你想复制的目录名!\n"); scanf("%s",&filename1); printf("请输入你复制后的目录名!\n"); scanf("%s",&filename2); cp3(filename2,1,0); cp2(filename1,filename2); } else { printf("输入有误 请重新运行后输入\n"); } return 0; }
bool MgPath::smoothBezierTo(const Point2d& cp2, const Point2d& end, bool rel) { Point2d lastpt(getEndPoint()); Point2d cp1(m_data->points.size() > 1 ? 2 * lastpt - m_data->points[m_data->points.size() - 2].asVector() : lastpt); m_data->points.push_back(cp1); m_data->points.push_back(rel ? cp2 + lastpt : cp2); m_data->points.push_back(rel ? end + lastpt : end); for (int i = 0; i < 3; i++) m_data->types.push_back(kMgBezierTo); return true; }
void ResampleCurve(const vector<double>& curvex, const vector<double>& curvey, vector<double>& resampleX, vector<double>& resampleY, int N, bool isOpen ) { assert(curvex.size()>0 && curvey.size()>0 && curvex.size() == curvey.size()); vector<Point2d> resamplepl(N); resamplepl[0].x = curvex[0]; resamplepl[0].y = curvey[0]; vector<Point2i> pl; PolyLineMerge(pl, curvex, curvey); double pl_length = arcLength(pl, true); double resample_size = pl_length / (double)N; int curr = 0; double dist = 0.0; for (int i = 1; i<N;) { assert(curr <(int)pl.size() - 1); double last_dist = norm(pl[curr] - pl[curr + 1]); dist += last_dist; // cout << curr << " and " << curr+1 << "\t\t" << last_dist << " ("<<dist<<")"<<endl; if (dist >= resample_size) { //put a point on line double _d = last_dist - (dist - resample_size); Point2d cp(pl[curr].x, pl[curr].y), cp1(pl[curr + 1].x, pl[curr + 1].y); Point2d dirv = cp1 - cp; dirv = dirv * (1.0 / norm(dirv)); // cout << "point " << i << " between " << curr << " and " << curr+1 << " remaining " << dist << endl; assert(i <(int)resamplepl.size()); resamplepl[i] = cp + dirv * _d; i++; dist = last_dist - _d; //remaining dist //if remaining dist to next point needs more sampling... (within some epsilon) while (dist - resample_size > 1e-3) { // cout << "point " << i << " between " << curr << " and " << curr+1 << " remaining " << dist << endl; assert(i <(int)resamplepl.size()); resamplepl[i] = resamplepl[i - 1] + dirv * resample_size; dist -= resample_size; i++; } } curr++; } PolyLineSplit(resamplepl, resampleX, resampleY); }
// Construct a Line NURBS by changing space dimension and ordering of //coordinates. MGRSBRep::MGRSBRep( size_t dim, // New space dimension. const MGRSBRep& rsb,// Original Surface B-rep. size_t start1, // Destination order of new line. size_t start2) // Source order of original line. :MGSurface(rsb){ update_mark(); size_t dim0=rsb.sdim(); MGSPointSeq cp1(dim0,rsb.surface_bcoef());//Exclude weights. MGSPointSeq cp2(dim,cp1,start1,start2); //Change order of coordinates. MGSPointSeq cp3(dim+1,cp2); //Get area for weights. for(size_t i=0; i<cp3.length_u(); i++) for(size_t j=0; j<cp3.length_v(); j++) cp3(i,j,dim)=rsb.surface_bcoef()(i,j,dim0);//Set weights. m_surface=MGSBRep(cp3,rsb.knot_vector_u(),rsb.knot_vector_v()); }
//Changing this object's space dimension. MGRSBRep& MGRSBRep::change_dimension( size_t dim, // new space dimension size_t start1, // Destination order of new object. size_t start2) // Source order of this object. { size_t dim0=sdim(); MGSPointSeq cp1(dim0,surface_bcoef()); //Exclude weights. MGSPointSeq cp2(dim,cp1,start1,start2); //Change order of coordinates. MGSPointSeq cp3(dim+1,cp2); //Get area for weights. const MGSPointSeq& sp=surface_bcoef(); for(size_t i=0; i<cp3.length_u(); i++) for(size_t j=0; j<cp3.length_v(); j++) cp3(i,j,dim)=sp(i,j,dim0);//Set weights. m_surface=MGSBRep(cp3,knot_vector_u(),knot_vector_v()); update_mark(); return *this; }
TEST(LockedRobotState, robotStateChanged) { moveit::core::RobotModelPtr model = getModel(); Super1 ls1(model); EXPECT_EQ(ls1.cnt_, 0); robot_state::RobotState cp1(*ls1.getState()); cp1.setVariablePosition(JOINT_A, 0.00001); cp1.setVariablePosition(JOINT_C, 0.00002); cp1.setVariablePosition(JOINT_F, 0.00003); ls1.setState(cp1); EXPECT_EQ(ls1.cnt_, 1); ls1.modifyState(modify1); EXPECT_EQ(ls1.cnt_, 2); ls1.modifyState(modify1); EXPECT_EQ(ls1.cnt_, 3); }
void testCase1(){ cow_ptr<string> cp1(new string("hello")); assert(*cp1 == "hello"); assert(cp1); auto cp2 = cp1; assert(*cp2 == "hello"); cow_ptr<string> cp3; cp3 = cp1; assert(*cp3 == "hello"); assert(cp1.get() == cp2.get() && cp2.get() == cp3.get()); assert(cp1 == cp2 && !(cp2 != cp3)); string zxh(" zouxiaohang"); cp1->append(zxh); assert(*cp1 == "hello zouxiaohang"); assert(*cp2 == "hello" && *cp3 == "hello"); cow_ptr<string> cp4; assert(cp4 == nullptr); }
// Check the state. It should always be valid. void MyInfo::checkState(robot_interaction::LockedRobotState &locked_state) { robot_state::RobotStateConstPtr s = locked_state.getState(); robot_state::RobotState cp1(*s); // take some time cnt_lock_.lock(); cnt_lock_.unlock(); cnt_lock_.lock(); cnt_lock_.unlock(); cnt_lock_.lock(); cnt_lock_.unlock(); // check mim_f == joint_f EXPECT_EQ(s->getVariablePositions()[MIM_F], s->getVariablePositions()[JOINT_F] * 1.5 + 0.1); robot_state::RobotState cp2(*s); EXPECT_NE(cp1.getVariablePositions(), cp2.getVariablePositions()); EXPECT_NE(cp1.getVariablePositions(), s->getVariablePositions()); int cnt = cp1.getVariableCount(); for (int i = 0 ; i < cnt ; ++i) { EXPECT_EQ(cp1.getVariablePositions()[i], cp2.getVariablePositions()[i]); EXPECT_EQ(cp1.getVariablePositions()[i], s->getVariablePositions()[i]); } // check mim_f == joint_f EXPECT_EQ(s->getVariablePositions()[MIM_F], s->getVariablePositions()[JOINT_F] * 1.5 + 0.1); }
void Game::init() { #ifdef WIN32 device = createDevice(video::EDT_DIRECT3D9, core::dimension2d<s32>(640, 480)); #else device = createDevice(video::EDT_OPENGL, core::dimension2d<s32>(640, 480)); #endif driver = device->getVideoDriver(); smgr = device->getSceneManager(); guienv = device->getGUIEnvironment(); device->setWindowCaption(L"Not So Much Super Offroad - By Andreas Kröhnke"); IMeshSceneNode *node = smgr->addMeshSceneNode( smgr->getMesh("data/car3.3ds")->getMesh(0) ); GameObject *car = new GameObject(); car->setSceneNode(node); node->setMaterialFlag(EMF_LIGHTING, false); node->setScale(vector3df(0.09,0.09,0.09)); node->setRotation(vector3df(0,0,0)); node->setPosition(vector3df(3454,500,1256)); //node->setDebugDataVisible(EDS_BBOX); object->push_back(car); ITerrainSceneNode *terrain = smgr->addTerrainSceneNode("data/level1.bmp"); GameObject *go_terrain = new GameObject(); go_terrain->setSceneNode(terrain); terrain->setMaterialFlag(EMF_LIGHTING, false); terrain->setScale(core::vector3df(18, 3.0f, 18)); terrain->setMaterialFlag(video::EMF_LIGHTING, false); terrain->setMaterialTexture(0, driver->getTexture("data/terrain-texture.jpg")); terrain->setMaterialTexture(1, driver->getTexture("data/detailmap3.jpg")); terrain->setMaterialType(video::EMT_DETAIL_MAP); terrain->scaleTexture(1.0f, 20.0f); terrain->setDebugDataVisible(EDS_BBOX); object->push_back(go_terrain); // Camera Camera *cam = new Camera(); cam->setSceneNode(smgr->addCameraSceneNode()); object->push_back(cam); cam->followNode(car->getSceneNode()); reciever = new EventReciever(); reciever->setSteer(car->getSceneNode()); device->setEventReceiver(reciever); // create triangle selector for the terrain ITriangleSelector* selector = smgr->createTerrainTriangleSelector(terrain, 0); terrain->setTriangleSelector(selector); selector->drop(); // create collision response animator and attach it to the camera ISceneNodeAnimator* anim = smgr->createCollisionResponseAnimator(selector, car->getSceneNode(), core::vector3df(10,10,10), core::vector3df(0,-5.0f,0), core::vector3df(0,0,0) ); car->getSceneNode()->addAnimator(anim); anim->drop(); // Skybox smgr->addSkyBoxSceneNode( driver->getTexture("data/irrlicht2_up.jpg"), driver->getTexture("data/irrlicht2_dn.jpg"), driver->getTexture("data/irrlicht2_lf.jpg"), driver->getTexture("data/irrlicht2_rt.jpg"), driver->getTexture("data/irrlicht2_ft.jpg"), driver->getTexture("data/irrlicht2_bk.jpg")); // Checkpoints pair<vector3df, vector3df> cp1(vector3df(3112,393,1234), vector3df(90,90,0)); addCheckPoint(cp1.first, cp1.second); pair<vector3df, vector3df> cp2(vector3df(2531,281,1389), vector3df(90,120,0)); addCheckPoint(cp2.first, cp2.second); addCheckPoint(vector3df(2304,160,1826), vector3df(90,140,0)); addCheckPoint(vector3df(2132,111,2672), vector3df(90,120,0)); addCheckPoint(vector3df(1130,415,3313), vector3df(90,75,0)); addCheckPoint(vector3df(746,471,1753), vector3df(90,0,0)); addCheckPoint(vector3df(1985,269,1457), vector3df(90,-120,0)); addCheckPoint(vector3df(2475,146,2868), vector3df(90,-120,0)); addCheckPoint(vector3df(3707,417,2915), vector3df(90,-60,0)); // Arrows addArrow(vector3df(3012,320,1234), vector3df(100,-55,0)); addArrow(vector3df(2531,220,1389), vector3df(100,-10,0)); //addArrow(vector3df(2304,110,1826), vector3df(90,10,0)); addArrow(vector3df(2232,20,2272), vector3df(90,-20,0)); // HUD info = guienv->addStaticText(L"USE ARROW KEYS TO PLAY", rect<int>(10,10,200,60), true); info->setOverrideColor(SColor(255, 255, 255, 255)); //IGUIStaticText *quick_info = guienv->addStaticText(L"Arrow keys to play\n", rect<int>(10,50,200,50), true); //quick_info->setOverrideColor(SColor(255,255,255,255)); initSound(); }
/*! Checks to see if the given AP is identical to itself ([this]). It also contains some useful points of instrumentation for benchmarking table and usage characteristics. \return TRUE, if and only if we match the AP given, false otherwise. */ bool PP_AttrProp::isExactMatch(const PP_AttrProp * pMatch) const { // The counters below are used in testing to profile call and chksum characteristics, // including collision rates. // NB: I'm not sure this initialization block is in the correct place. #ifdef PT_TEST static UT_uint32 s_Calls = 0; static UT_uint32 s_PassedCheckSum = 0; static UT_uint32 s_Matches = 0; #endif #ifdef PT_TEST s_Calls++; #endif UT_return_val_if_fail (pMatch, false); // // Why is this here? Nothing is being changed? // UT_return_val_if_fail (m_bIsReadOnly && pMatch->m_bIsReadOnly, false); if (m_checkSum != pMatch->m_checkSum) return false; #ifdef PT_TEST s_PassedCheckSum++; #endif UT_uint32 countMyAttrs = ((m_pAttributes) ? m_pAttributes->size() : 0); UT_uint32 countMatchAttrs = ((pMatch->m_pAttributes) ? pMatch->m_pAttributes->size() : 0); if (countMyAttrs != countMatchAttrs) return false; UT_uint32 countMyProps = ((m_pProperties) ? m_pProperties->size() : 0); UT_uint32 countMatchProps = ((pMatch->m_pProperties) ? pMatch->m_pProperties->size() : 0); if (countMyProps != countMatchProps) return false; if (countMyAttrs != 0) { UT_GenericStringMap<gchar*>::UT_Cursor ca1(m_pAttributes); UT_GenericStringMap<gchar*>::UT_Cursor ca2(pMatch->m_pAttributes); const gchar * v1 = ca1.first(); const gchar * v2 = ca2.first(); do { const gchar *l1 = ca1.key().c_str(); const gchar *l2 = ca2.key().c_str(); if (strcmp(l1, l2) != 0) return false; l1 = v1; l2 = v2; if (strcmp(l1,l2) != 0) return false; v1 = ca1.next(); v2 = ca2.next(); } while (ca1.is_valid() && ca2.is_valid()); } if (countMyProps > 0) { UT_GenericStringMap<PropertyPair*>::UT_Cursor cp1(m_pProperties); UT_GenericStringMap<PropertyPair*>::UT_Cursor cp2(pMatch->m_pProperties); const PropertyPair* v1 = cp1.first(); const PropertyPair* v2 = cp2.first(); do { const gchar *l1 = cp1.key().c_str(); const gchar *l2 = cp2.key().c_str(); if (strcmp(l1, l2) != 0) return false; l1 = v1->first; l2 = v2->first; if (strcmp(l1,l2) != 0) return false; v1 = cp1.next(); v2 = cp2.next(); } while (cp1.is_valid() && cp2.is_valid()); #ifdef PT_TEST s_Matches++; #endif } return true; }
double SegSegDist(const hrp::Vector3& u0, const hrp::Vector3& u1, const hrp::Vector3& v0, const hrp::Vector3& v1) { hrp::Vector3 u(u1 - u0); hrp::Vector3 v(v1 - v0); hrp::Vector3 w(u0 - v0); double a = u.dot(u); // always >= 0 double b = u.dot(v); double c = v.dot(v); // always >= 0 double d = u.dot(w); double e = v.dot(w); double D = a*c - b*b; // always >= 0 double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 // compute the line parameters of the two closest points #define EPS 1e-8 if (D < EPS) { // the lines are almost parallel sN = 0.0; // force using point P0 on segment S1 sD = 1.0; // to prevent possible division by 0.0 later tN = e; tD = c; } else { // get the closest points on the infinite lines sN = (b*e - c*d); tN = (a*e - b*d); if (sN < 0.0) { // sc < 0 => the s=0 edge is visible sN = 0.0; tN = e; tD = c; } else if (sN > sD) { // sc > 1 => the s=1 edge is visible sN = sD; tN = e + b; tD = c; } } if (tN < 0.0) { // tc < 0 => the t=0 edge is visible tN = 0.0; // recompute sc for this edge if (-d < 0.0) sN = 0.0; else if (-d > a) sN = sD; else { sN = -d; sD = a; } } else if (tN > tD) { // tc > 1 => the t=1 edge is visible tN = tD; // recompute sc for this edge if ((-d + b) < 0.0) sN = 0; else if ((-d + b) > a) sN = sD; else { sN = (-d + b); sD = a; } } // finally do the division to get sc and tc sc = (fabsf(sN) < EPS ? 0.0f : sN / sD); tc = (fabsf(tN) < EPS ? 0.0f : tN / tD); hrp::Vector3 cp0(u0 + sc * u); hrp::Vector3 cp1(v0 + tc * v); // get the difference of the two closest points hrp::Vector3 dP(cp0 - cp1); return dP.norm(); // return the closest distance }