// 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
}
