自学教程：C++ DBGP函数代码示例

void ArizonaTest::initializeTest(){	if(!mObject){		DBGA("ArizonaTest: object not present");		return;	}	if (mContacts.empty()) {		DBGA("ArizonaTest: no contacts");		return;	}	std::vector<int> arizonaTestD(6,0);	if (mIsBuildIn3D) {		//fx, tx and ty		arizonaTestD[2] = arizonaTestD[3] = arizonaTestD[4] = 1;	} else {		arizonaTestD = Grasp::ALL_DIMENSIONS;	}	DBGP("AT: updating grasp");	mGrasp->update(arizonaTestD); //  arizonaTestD is used when isBuildIn3D = true;	//create the 3D hull that we are interested in 	DBGP("AT: creating 3D hull");	createGWSProjection();	if(mQual->evaluate() < 0){		DBGA("Non-ForceClosure");		return;	}	for(int i = 1; i <= NUMCMPATTERN; i ++){		forceScales[i-1] = getMinimunForce(i);	}	DBGP("AT: all tests initialized");}

/* create directory recursively */int mkdir_p(const char *path){	int n;	char buf[PATH_MAX], *p = buf;	assert(path);	if ((n = strlen(path)) >= PATH_MAX) {		DBGP("over length: %d, %d", n, PATH_MAX);		return -1;	}	DBGP("len=%d; path='%s'/n", n, path);	strcpy(buf, path);	while ((p = strchr(p+1, '/'))) {		struct stat sb;		*p = '/0';		if (stat(buf, &sb) && mkdir(buf, ACCESSPERMS)) {			DBGE("stat/create '%s' failed", buf);			return -1;		}		*p = '/';	}	return n;}

/*! For elastic bodies, calculates the contact neighborhoods and puts 	them in the contact set. Not done for rigid contacts.*/void findSoftNeighborhoods( Body *body1, Body *body2, ContactReport &contactSet ){	ContactReport::iterator itr;	for( itr = contactSet.begin(); itr != contactSet.end(); itr++ )	{		DBGP("Contact finding regions:");		//right now, findregion assumes point is in body frame		//the units for the threshold and the radius should be in mm, NOT cm		//The input radius is proportional to the fourth root of the youngs mod/depth of mattress		//(units for Youngs Mod and mattress depth are in Pa and meters)		//This gives a radius around 6 mm, for rubber with youngs = 1.5E6 and h = 3E-3which is reasonable		double rad = pow( 1/(MAX( body1->getYoungs(), body2->getYoungs() )), 0.333 ) * 1000.0 * 0.4;		//the 0.4 is a fudge factor for the time being		//hack to ensure that the fit is at least resonable		if( rad <= 3.0 && rad >= 10.0 )	rad = 5.0;		body1->getWorld()->FindRegion( body1, itr->b1_pos, itr->b1_normal, rad, &(itr->nghbd1) );		DBGP("Neighborhood on body1 has " << itr->nghbd1.size() << " points");		body2->getWorld()->FindRegion( body2, itr->b2_pos, itr->b2_normal, rad, &(itr->nghbd2) );		DBGP("Neighborhood on body2 has " << itr->nghbd2.size() << " points");	}}

bool HandObjectState::readFromFile(FILE *fp){	//this whole read-write system is one big hack.	int type; fpos_t pos;	fgetpos(fp,&pos);	if (!fscanf(fp,"%d",&type)) return false;	DBGP("Pose type: " << type);	if ( (StateType)type != POSE_DOF && (StateType)type != POSE_EIGEN ) return false;	if ( type != mPosture->getType() ) {		setPostureType((StateType)type);	}	fsetpos(fp,&pos);	if ( !mPosture->readFromFile(fp) ) {		DBGA("Failed");		return false;	}	fgetpos(fp,&pos);	if (!fscanf(fp,"%d",&type)) return false;	DBGP("Space type: " << type);	if ( (StateType)type != SPACE_COMPLETE && (StateType)type != SPACE_APPROACH &&		 (StateType)type != SPACE_AXIS_ANGLE && (StateType)type != SPACE_ELLIPSOID ) return false;	if ( type != mPosition->getType() ) {		setPositionType((StateType)type);	}	fsetpos(fp,&pos);	if ( !mPosition->readFromFile(fp) ) return false;	return true;}

voidGraspTester::mainLoop(){    GraspPlanningState *s = popCandidate();    if (!s) {        DBGP("Empty buffer for tester");        msleep(100);        return;    }    s->changeHand(mHand, true);    testGrasp(s);    DBGP("TESTER: candidate has energy " << s->getEnergy());    mHand->breakContacts();    if (s->isLegal() && s->getEnergy() < -1.2) {        //save the final grasping position that has resulted from autograsp        s->setPositionType(SPACE_COMPLETE);        s->setPostureType(POSE_DOF);        //save the current transform in absolute terms        s->setRefTran(transf::IDENTITY);        s->saveCurrentHandState();        postSolution(s);        DBGP("Tester posting a solution at iteration " << s->getItNumber());    } else {        DBGP("Tester removing candidate");        delete s;    }}

/*! Given a solution (grasping position) this function finds  a correct finger posture for the CURRENT position of the  hand, so that it's as close as possible to the solution  without hitting the object. It also checks if there is a  path between the CURRENT position of the fingers and the  desired one.*/boolOnLineGraspInterface::getSuggestedDOF(const GraspPlanningState *s, double *initialDof, double *finalDof){  s->readPosture()->getHandDOF(finalDof);  mHand->forceDOFVals(finalDof);  //close fingers gradually as we move closer to the target state  transf handTran = mHand->getTran();  transf solTran = s->getTotalTran();  vec3 app = handTran.translation() - solTran.translation();  double dist = app.len();  //first find how much we should open the fingers, based on distance from solution  double openFingers = dist / 200.0;  DBGP("Open fingers to " << openFingers);  if (!mHand->quickOpen(openFingers)) {    DBGP("Open finger position not found");    return false;  }  mHand->getDOFVals(finalDof);  //also check if we can get from here to there  mHand->forceDOFVals(initialDof);  if (mHand->checkDOFPath(finalDof, 0.16)) {    return true;  } else {    DBGP("Open finger found, but not reachable");    return false;  }}

/*! Attempts to maintain a list of unique solutions. Therefore, whenever  a new state is added to the list, we check if any of the states that  are already in the list are within a given distance of the new state.  If so, the best one is kept and the other one is thrown away. This  method does not gurantee unique states, but it comes close and runs  in linear time for each addition, rather than square time for  maintenance.*/boolEGPlanner::addToListOfUniqueSolutions(GraspPlanningState *s, std::list<GraspPlanningState *> *list, double distance){    std::list<GraspPlanningState *>::iterator it;    it = list->begin();    bool add = true;    while (it != list->end()) {        double d = stateDistance(s, *it);        if (fabs(d) < distance) {            DBGP("Distance: " << fabs(d));            //states are close to each other            if (s->getEnergy() < (*it)->getEnergy()) {                //new state is better; remove old one from list                delete(*it);                it = list->erase(it);                DBGP("Old state removed");            } else {                //old state is better; we don't want to add the new one                add = false;                break;            }        } else {            //states are not close, proceed through the list            it++;        }    }    if (add) {        list->push_back(s);    }    return add;}

/* * Increments which cycles are cut. If all combinations have been tried, fixes * the graph and returns 0. Otherwise returns 1. */int inc_cycles(cycle_counter counter, graph* g, graph* f) {   ll_node* cc = counter->counter;   int carry = 0;   do {      ll_node* cycle = (ll_node*)cc->data;      cc_node n = *(cc_node*)cycle->data;      fix_edge(counter->g_info,g->verts + n.src,n.g_cc_i);      fix_edge(counter->f_info,f->verts + n.dest,n.f_cc_i);      if(cycle->next->data == NULL) {         carry = 1;         cc->data = cycle->next->next;         n = *(cc_node*)cycle->next->next->data;      } else {         cc->data = cycle->next;         n = *(cc_node*)cycle->next->data;      }      DBGP(1);      DBGP(2);      DBGP(3);      delete_edge(counter->g_info,g->verts + n.src,n.g_cc_i);      delete_edge(counter->f_info,f->verts + n.dest,n.f_cc_i);      cc = cc->next;   } while(carry == 1 && cc != NULL);   return carry != 1;}

void CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, 										 std::list<GraspPlanningState*> *sampling, 										 int samples, double x, double y, double z){	//redundant, but easier...	double a = seed.readPosition()->getParameter("a");	double b = seed.readPosition()->getParameter("b");	//double c = seed.readPosition()->getParameter("c");	//compute angular values	//from HandObjectStateImpl:	//x =  a * cos(beta) * cos(gamma);	//y =  b * cos(beta) * sin(gamma);	//z =  c * sin(beta);	double beta = asin(z / sqrt(x*x + y*y + z*z));	double gamma = atan2(y/b, x/a);	DBGP("x: " << x << "; y: " << y <<"; z: " << z);	DBGP("gamma: " << gamma << "; beta: " << beta);	//sample roll angle as well	for (int m=0; m<samples; m++) {		//only sample from 0 to almost PI, as the HH is symmetric		double tau = M_PI * ((double)m) / samples;		GraspPlanningState *newState = new GraspPlanningState(&seed);		newState->getPosition()->getVariable("tau")->setValue(tau);		newState->getPosition()->getVariable("gamma")->setValue(gamma);		newState->getPosition()->getVariable("beta")->setValue(beta);		sampling->push_back(newState);	}}

/*!  After a timestep has been completed, this computes the current joint angle  and velocity from the relative positions and velocities of the connected  links.*/voidRevoluteDynJoint::updateValues(){  transf b1JointTran = prevFrame * prevLink->getTran();  //the z axis of the previous link, by definition the axis of the one joint  vec3 axis = b1JointTran.affine().row(2);  joint->setWorldAxis(axis);  double vel1 = vec3(prevLink->getVelocity()[3],					 prevLink->getVelocity()[4],					 prevLink->getVelocity()[5]) % axis;  double vel2 = vec3(nextLink->getVelocity()[3],					 nextLink->getVelocity()[4],					 nextLink->getVelocity()[5]) % axis;  joint->setVelocity(vel2-vel1);  transf diffTran = joint->getTran(0.0).inverse() * nextLink->getTran() * b1JointTran.inverse();   double val;  diffTran.rotation().ToAngleAxis(val,axis);  if (axis.z() < 0) val = -val;    DBGP("link "<< prevLink->getName().latin1() <<" - link "<<nextLink->getName().latin1());  DBGP(" joint angle: "<<val*180.0/M_PI<<" radians: "<<val<< " velocity: "<<vel2-vel1);  joint->setDynamicsVal(val);}

void Leaf::fitBox(const mat3 &R, vec3 &center, vec3 &halfSize){	vec3 x = R.row(0);	vec3 y = R.row(1);	vec3 z = R.row(2);	vec3 max(-1.0e10, -1.0e10, -1.0e10);	vec3 min( 1.0e10,  1.0e10,  1.0e10);	std::list<Triangle>::iterator it;	for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {		boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);		boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);		boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);	}	DBGP("Max: " << max);	DBGP("Min: " << min);	for (int i=0; i<3; i++) {		halfSize[i] = 0.5 * (max[i] - min[i]);	}	DBGP("computed halfsize: " << halfSize);	//halfSize = 0.5 * (max - min);	center = min + halfSize;	center = R.inverse() * center;	//sanity check	for (int i=0; i<3; i++) {		if (halfSize[i] < TOLERANCE) {			if (halfSize[i] < 0.5 * TOLERANCE) {				DBGA("Warning: degenerate box computed");			}			halfSize[i] = TOLERANCE;		}	}	DBGP("returned halfsize: " << halfSize);}

static int piapidev_parse( const char *initstr, unsigned int *saddr, unsigned int *sport ){    int shift = 24;    char *token;    DBGP( "Info: received initialization string %s/n", initstr );    *saddr = 0;    while( shift >= 0 ) {        if( (token = strtok( (shift!=24) ? NULL : (char *)initstr, (shift!=0) ? "." : ":" )) == 0x0 ) {            fprintf( stderr, "Error: invalid server IP address in initialization string %s/n", initstr );            return -1;        }        *saddr |= ( atoi(token) << shift );        shift -= 8;    }    if( (token = strtok( NULL, ":" )) == 0x0 ) {        fprintf( stderr, "Error: missing server port separator in initialization string %s/n", initstr );        return -1;    }    *sport = atoi(token);    DBGP( "Info: extracted initialization string (SADDR=%08x, SPORT=%u)/n", *saddr, *sport );    return 0;}

double ArizonaTest::getQuality(){	DBGP("AT: evaluating quality");	double q = mQual->evaluate();	DBGP("Quality: " << q);	if (q < 0) q = -1;	return q;}

void Node::getBVRecurse(int currentDepth, int desiredDepth, std::vector<BoundingBox> *bvs){	if (currentDepth == desiredDepth || isLeaf() ) {		bvs->push_back( BoundingBox(mBbox) );		DBGP("BBox tran: " << mBbox.getTran());		DBGP("BBox size: " << mBbox.halfSize);		return;	}}

/*!  After a timestep has been completed, this computes the current joint angles  and velocities from the relative positions and velocities of the connected  links.*/voidUniversalDynJoint::updateValues(){  vec3 axis,ax0,ax1,ax2;  double val,vel1,vel2;  transf b1JointTran = prevFrame * prevLink->getTran();  transf b2JointTran = nextFrame * nextLink->getTran();  // the z axis of the previous link - by definition, the rotation direction of the next joint  ax0 = b1JointTran.affine().row(2);  ax2 = b2JointTran.affine().row(2);  ax1 = normalise(ax2*ax0);  DBGP("ax0: "<<ax0<<" len "<<ax0.len());  DBGP("ax1: "<<ax1<<" len "<<ax1.len());  DBGP("ax2: "<<ax2<<" len "<<ax2.len());  axis = ax1*ax2;  joint1->setWorldAxis(axis);  vel1 = vec3(prevLink->getVelocity()[3],			  prevLink->getVelocity()[4],			  prevLink->getVelocity()[5]) % axis;  vel2 = vec3(nextLink->getVelocity()[3],			  nextLink->getVelocity()[4],			  nextLink->getVelocity()[5]) % axis;  joint1->setVelocity(vel2-vel1);  vec3 ref1 = (joint2->getTran(0.0)*joint1->getTran(0.0)*b1JointTran).affine().row(2);  //original GraspIt:  val = atan2 (ax2 % (ax0 * ref1), ax2 % ref1);   DBGP("link " << prevLink->getName().latin1() << " - link " << nextLink->getName().latin1() << ":");  DBGP("   joint1 angle: " << val*180.0/M_PI << " " << val << " (rad)");  joint1->setDynamicsVal(val);    //is this right here? It's different from what's done for joint1  axis = b2JointTran.affine().row(2);  //joint2->setWorldAxis(axis);  joint2->setWorldAxis(ax0*ax1);    vel1 = vec3(prevLink->getVelocity()[3],			  prevLink->getVelocity()[4],			  prevLink->getVelocity()[5]) % axis;  vel2= vec3(nextLink->getVelocity()[3],			 nextLink->getVelocity()[4],			 nextLink->getVelocity()[5]) % axis;    joint2->setVelocity(vel2-vel1);    vec3 ref2 = (joint2->getTran(0.0)*joint1->getTran(0.0)).inverse().affine().row(2) * b2JointTran;  val = atan2 (ref2 % ax1, ref2 % (ax1*ax2));  joint2->setDynamicsVal(val);   }

/*! This formulation combines virtual contact energy with autograsp energy. Virtual contact energy is used to "guide"    initial stages of the search and to see if we should even bother computing autograsp quality. Autograsp is a couple    of orders of magnitude higher and so should work very well with later stages of the sim ann search*/doubleGuidedAutoGraspQualityEnergy::energy() const{  //first compute regular contact energy; also count how many links are "close" to the object  VirtualContact *contact;  vec3 p, n, cn;  double virtualError = 0; int closeContacts = 0;  //collect virtual contacts first  mHand->getGrasp()->collectVirtualContacts();  for (int i = 0; i < mHand->getGrasp()->getNumContacts(); i++) {    contact = (VirtualContact *)mHand->getGrasp()->getContact(i);    contact->getObjectDistanceAndNormal(mObject, &p, &n);    double dist = p.len();    if ((-1.0 * p) % n  < 0) { dist = -dist; }    //BEST WORKING VERSION, strangely enough    virtualError += fabs(dist);    cn = -1.0 * contact->getWorldNormal();    double d = 1 - cn % n;    virtualError += d * 100.0 / 2.0;    if (fabs(dist) < 20 && d < 0.3) { closeContacts++; }  }  virtualError /= mHand->getGrasp()->getNumContacts();  //if more than 2 links are "close" go ahead and compute the true quality  double volQuality = 0, epsQuality = 0;  if (closeContacts >= 2) {    mHand->autoGrasp(false, 1.0);    //now collect the true contacts;    mHand->getGrasp()->collectContacts();    if (mHand->getGrasp()->getNumContacts() >= 4) {      mHand->getGrasp()->updateWrenchSpaces();      volQuality = mVolQual->evaluate();      epsQuality = mEpsQual->evaluate();      if (epsQuality < 0) { epsQuality = 0; } //QM returns -1 for non-FC grasps    }    DBGP("Virtual error " << virtualError << " and " << closeContacts << " close contacts.");    DBGP("Volume quality: " << volQuality << " Epsilon quality: " << epsQuality);  }  //now add the two such that the true quality is a couple of orders of magn. bigger than virtual quality  double q;  if (volQuality == 0) { q = virtualError; }  else { q = virtualError - volQuality * 1.0e3; }  if (volQuality || epsQuality) {DBGP("Final quality: " << q);}  //DBGP("Final value: " << q << std::endl);  return q;}

doubleSimAnn::biasedNeighborDistribution(double T, double in, double conf){  double u1, u2, u;  double sigma_sq, mean;  //we get the confidence as a linear value between 0 and 1  //if we don't invert the nbr fct first, a variance of 0.1 seems to be "average confidence"  //convert to a log so that we exploit mainly the area between 0 and 0.1  //this also inverts it, mapping conf = 1 (max confidence) to sigma = 0 (always produce a 0)  //if (conf < 0.01) sigma_sq = 1;  //else sigma_sq = -log( conf ) / 10;  //for the case where we use the inverse neighbor, it's best to set the variance more conservatively  //which means that most play is actually between 0.1 and 1  //so all we gotta do is invert the confidence  sigma_sq = 1.0 - conf;  DBGP("In: " << in << " conf: " << conf << " simga_sq: " << sigma_sq);  //the target value we get here is normalized and capped to -1..1  //we set the mean of the gaussian to the value that the neighbor function would map to our target value  mean = neighborInverse(T, in);  //alternatively, we could just set it directly to the target and let the neighbor fctn map it to  //something depending on the temperature, like this:  //mean = in;  //but this means that even for infinitely strict target (confidence = 1.0), we can never reach it exactly  //as the nbr fctn will map it to smaller and smaller steps.  //compute a normal distribution, centered at input and clamped btw -1 and 1  u = 2;  double loops = 0;  while (u > 1 || u < -1) {    //start with uniform distribution    u1 = ((double)rand()) / RAND_MAX;    u2 = ((double)rand()) / RAND_MAX;    //turn it into a normal distribution with the Box-Muller transform    u = sqrt(-2 * log(u1)) * cos(2 * M_PI * u2);    //set variance based on confidence and mean based on input    u = mean + sqrt(sigma_sq) * u;    loops++;  }  //just check that we're not spending too much time here  if (loops > 20) { DBGA("Biased distribution loops:  " << loops); }  //use it to generate Ingber neighbor  double y = T * (pow(1.0 + 1.0 / T , fabs(u)) - 1);  if (u < 0) { y = -1.0 * y; }  DBGP("u: " << u << " y: " << y << " loops: " << loops);  return y;}

/*! Sets the max and min vals of the DOF from the smallest range of the joint   limits. Also forces defaultValue to be inside those limits.*/void DOF::updateMinMax(){  maxq = (*jointList.begin())->getMax()/getStaticRatio(*jointList.begin());  minq = (*jointList.begin())->getMin()/getStaticRatio(*jointList.begin());  DBGP("Joint 0 min "  << minq << " max " << maxq);  if (maxq < minq) std::swap(maxq, minq);    DBGP("maxq " << maxq << " minq " << minq);  std::vector<Joint *>::iterator j;  double testMin, testMax;  DBGST(int num = 0;)  for(j=++jointList.begin();j!=jointList.end();j++) {

voidJoint::applyPassiveInternalWrenches(){	double f = getFriction();	DBGP("Friction coeffs: " << f0 << " " << f1);	DBGP("Friction force: " << f);	if (f != 0.0) applyInternalWrench(f);	f = getSpringForce();	DBGP("Spring force: " << f);	applyInternalWrench(-f);}

double AutoGraspQualityEnergy::energy() const{  DBGP("Autograsp quality computation");  mHand->autoGrasp(false, 1.0);  mHand->getGrasp()->collectContacts();  mHand->getGrasp()->updateWrenchSpaces();  double volQual = mVolQual->evaluate();  double epsQual = mEpsQual->evaluate();  if (epsQual < 0) { epsQual = 0; } //returns -1 for non-FC grasps  DBGP("Autograsp quality: " << volQual << " volume and " << epsQual << " epsilon.");  return - (30 * volQual) - (100 * epsQual);}

double ArizonaTest::binarySearch(const vec3 &direction, PQP_Model &volume, double startValue, double endValue){	//we assume that startValue is inside the model and endValue is outside the model	//we find the value in between that is exactly on the model	double currentValue = startValue;	double currentInterval = (endValue - startValue)/2.0;	PQP_REAL pt[3];	PQP_REAL R[3][3]={{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}};	PQP_REAL T[3]={0.0,0.0,0.0};	double closest_dist = 1.0e9, thresh = 0.0, distance;	PQP_REAL closest_pt[3], closest_normal[3];	bool success = false;	while (1) {		pt[0] = currentValue * direction.x();		pt[1] = currentValue * direction.y();		pt[2] = currentValue * direction.z();		distance = GetShortestDist(pt, &volume, R, T,  closest_dist, closest_pt, closest_normal, thresh);		DBGP("disturbance: " << pt[0] << " " << pt[1] << " " << pt[2]);		DBGP("Closest point: " << closest_pt[0] << " " << closest_pt[1] << " " << closest_pt[2] << "; Distance is: " << distance);		DBGP("Current interval: " << currentInterval);		if(distance < EPSILON){			success = true;			break;		}		if(isOutside(&volume, pt)){ 			DBGP("outside" << std::endl);			if (currentValue <= startValue) {				DBGP("Error: startValue is outside of model!");				break;			}			currentValue -= currentInterval;		} else {			DBGP("inside" << std::endl);			if (currentValue >= endValue) {				DBGP("Error: endValue is inside model!");				break;			}			currentValue += currentInterval;		}		currentInterval /= 2;		if ( fabs(currentInterval) < 1.0e-10 ) {			DBGP("Max loops exceeded!");			break;		}	}	if (distance > 10e-5) {		DBGA("Binary search failed! distance is " << distance);	} else {		DBGP("Binary search success!");	}	return currentValue;}

/*! If the two paths have no common root, the returned path is  identical to /a absolutePath.  Code adapted from;  http://mrpmorris.blogspot.com/2007/05/convert-absolute-path-to-relative-path.html*/QStringrelativePath(QString absolutePath, QString relativeToDir){  absolutePath.replace("//", "/");  relativeToDir.replace("//", "/");  QStringList absoluteDirectories = absolutePath.split('/', QString::SkipEmptyParts);  QStringList relativeDirectories = relativeToDir.split('/', QString::SkipEmptyParts);  //Get the shortest of the two paths  int length = std::min(absoluteDirectories.count(), relativeDirectories.count());  //Use to determine where in the loop we exited  int lastCommonRoot = -1;  int index;  DBGP("Absolute path: " << absolutePath.latin1());  DBGP("Relative to  : " << relativeToDir.latin1());  //Find common root  for (index = 0; index < length; index++) {    if (absoluteDirectories[index] == relativeDirectories[index]) {      lastCommonRoot = index;    } else {      break;    }  }  DBGP("Last common root: " << lastCommonRoot);  //If we didn't find a common prefix then return full absolute path  if (lastCommonRoot == -1) {    return absolutePath;  }  //Build up the relative path  QString relativePath;  //Add on the ..  for (index = lastCommonRoot + 1; index < relativeDirectories.count(); index++) {    if (relativeDirectories[index].length() > 0) {      relativePath.append("../");    }  }  //Add on the folders  for (index = lastCommonRoot + 1; index < absoluteDirectories.count() - 1; index++) {    relativePath.append(absoluteDirectories[index]).append("/");  }  relativePath.append(absoluteDirectories[absoluteDirectories.count() - 1]);  DBGP("Relative path: " << relativePath.latin1());  return relativePath;}

/*!  Given a pointer to the GWS to remove, this decrements the reference count  for that GWS.  If the reference count becomes 0, the GWS is deleted.*/voidGrasp::removeGWS(GWS *gws){  DBGP("removing gws");  gws->unref();  if (gws->getRefCount() == 0) {    DBGP("deleting gws");    gwsList.remove(gws);    delete gws;  } else {	  DBGP("gws refcount: "<<gws->getRefCount());  }}

/*! Computes an analytical surface of the form ax^2 + bx + c in a small patch	around the contact on body1. The fit is in the local body1 coordinate 	system.*/void SoftContact::FitPoints( ){	double *coeffs = new double [3];	FitParaboloid( bodyNghbd, numPts, coeffs );	a = coeffs[0];	b = coeffs[1];	c = coeffs[2];	RotateParaboloid( coeffs, &r1, &r2, &fitRot, &fitRotAngle );	DBGP(getBody1()->getName().latin1() << ": " << "a=" << a << " b=" << b << " c=" <<c);	DBGP("r1=" << r1 << " r2=" <<r2);}

voidSimAnn::variableNeighbor(VariableSet *set, double T, VariableSet *target){  SearchVariable *var;  double v, tv = 0, conf = 0;  for (int i = 0; i < set->getNumVariables(); i++) {    var = set->getVariable(i);    if (var->isFixed()) { continue; }    v = var->mMaxVal + 1.0; //start off ilegal    int loop = 0;    while (v > var->mMaxVal || v < var->mMinVal) {      loop++;      if (!target || !target->getVariable(i)->isFixed()) {        //we have no target value; use regular sim ann neighbor distribution        v = var->getValue() + neighborDistribution(T) * var->mMaxJump;      } else {        //we have a target value and a confidence level        tv = target->getVariable(i)->getValue();        DBGP(target->getVariable(i)->getName().toStdString().c_str() << " input: " << tv);        conf = target->getVariable(i)->getConfidence();        assert(conf >= 0 && conf <= 1);        //normalize desired change to -1..1 interval relative to the max jump        double change = tv - var->getValue();        if (change > var->mMaxJump) { change = var->mMaxJump; }        else if (change < -1 * var->mMaxJump) { change = -1 * var->mMaxJump; }        change = change / var->mMaxJump;        //call the appropriate neighbor generator        DBGP(var->getName().toStdString().c_str() << " value: " << var->getValue() << " Target: " << tv << " Change: " << change);        v = var->getValue() + biasedNeighborDistribution(T, change, conf) * var->mMaxJump;      }      if (var->isCircular()) {        DBGP("Circular variable! " << var->getName().toStdString().c_str());        if (v > var->mMaxVal) { v -= var->getRange(); }        else if (v < var->mMinVal) { v += var->getRange(); }      }      if (v > var->mMaxVal && v - var->mMaxVal < TINY) { v = var->mMaxVal; }      if (v < var->mMinVal && v - var->mMinVal > -TINY) { v = var->mMinVal; }      if (loop == 100) {        DBGA("value: " << var->getValue() << " Mj: " << var->mMaxJump);        DBGA("min val: " << var->mMinVal << " max val: " << var->mMaxVal);        if (target->getVariable(i)->isFixed()) {          DBGA("Target: " << tv << "; Nbr: " << biasedNeighborDistribution(T, tv - var->getValue(), conf));        }        break;      }    }    if (loop > 10) { DBGA("Neighbor gen loops: " << loop); }    var->setValue(v);  }}

int pwr_piapidev_readv( pwr_fd_t fd, unsigned int arraysize,    const PWR_AttrName attrs[], void *values, PWR_Time timestamp[], int status[] ){    unsigned int i;    while( piapidev_reading ) sched_yield();    piapidev_reading = 1;    DBGP( "Info: reading counter for port %d/n", PWR_PIAPIFD(fd)->port );    if( piapi_counter( (PWR_PIAPIFD(fd)->dev)->cntx, PWR_PIAPIFD(fd)->port ) < 0 ) {        fprintf( stderr, "Error: powerinsight hardware read failed/n" );        return -1;    }    while( piapidev_reading ) sched_yield();    for( i = 0; i < arraysize; i++ ) {        switch( attrs[i] ) {            case PWR_ATTR_VOLTAGE:                *((double *)values+i) = (double)piapidev_counter.raw.volts;                break;            case PWR_ATTR_CURRENT:                *((double *)values+i) = (double)piapidev_counter.raw.amps;                break;            case PWR_ATTR_POWER:                *((double *)values+i) = (double)piapidev_counter.raw.watts;                break;            case PWR_ATTR_POWER_LIMIT_MIN:                *((double *)values+i) = (double)piapidev_counter.min.watts;                break;            case PWR_ATTR_POWER_LIMIT_MAX:                *((double *)values+i) = (double)piapidev_counter.max.watts;                break;            case PWR_ATTR_ENERGY:                *((double *)values+i) = (double)piapidev_counter.energy;                break;            default:                fprintf( stderr, "Warning: unknown PWR reading attr (%u) requested/n", attrs[i] );                break;        }        timestamp[i] = piapidev_counter.time_sec*1000000000ULL +              piapidev_counter.time_usec*1000;        DBGP( "Info: reading of type %u at time %llu with value %lf/n",            attrs[i], *((unsigned long long *)timestamp+i), *((double *)values+i) );        status[i] = pwr_piapidev_read( fd, attrs[i], (double *)values+i, sizeof(double), timestamp+i );    }    return 0;}

/*! After this is called, the planner can no longer be re-started. 	This differentiation is needed mainly for the multi-threaded case: 	this function stops the planner's thread.*/voidEGPlanner::stopPlanner(){	if (getState()==DONE || getState()==EXITED) return;	//this will stop the planner REGARDLESS of what state it is in!	pausePlanner();	//this also finishes the thread	setState(DONE);	if (mMultiThread) {		DBGP("Waiting for exit");		//wait for the thread to stop spinning		while (getState()!=EXITED);		DBGP("Exited");	}}

void CollisionInterface::compactContactSet(ContactReport *contacts){	if (contacts->size() < 2) return;	DBGP("Compacting total contacts: " << contacts->size());	double NORMAL_TOLERANCE = 1e-3;	double DISTANCE_TOLERANCE = 1.0e-1;	//this will hold all the groups of contacts that have the same normal	std::list<ContactReport> contactGroups;	//create lists of contacts with the same normal	ContactReport::iterator cp;	std::list<ContactReport>::iterator sp;	for (cp=contacts->begin(); cp!=contacts->end(); cp++) {		for (sp=contactGroups.begin(); sp!=contactGroups.end(); sp++) {			if ( sp->begin()->b1_normal % cp->b1_normal > 1.0-NORMAL_TOLERANCE) break;		}		if (sp == contactGroups.end()) {			contactGroups.push_back(ContactReport());			contactGroups.back().push_back(*cp);		} else {			// we need to make sure it's not in there already since it's a list, not a set			ContactReport::iterator it;			for (it=sp->begin(); it!=sp->end(); it++) {				vec3 dist = it->b1_pos - cp->b1_pos;				if ( dist.len_sq() < DISTANCE_TOLERANCE) break;			}			if (it==sp->end()) sp->push_back(*cp);		}	} 	//take perimeter of sets with same normal	for (sp=contactGroups.begin(); sp!=contactGroups.end(); sp++) {		DBGP("Set with same normal: " << sp->size());		if (sp->size() > 1) replaceContactSetWithPerimeter(*sp);		DBGP("Perimeter contacts: " << sp->size());	}	//insert compacted sets back in result	contacts->clear();	for (sp=contactGroups.begin(); sp!=contactGroups.end(); sp++) {		for (cp=sp->begin();cp!=sp->end();cp++) {			contacts->push_back(*cp);		}	}}

voidEGPlanner::render(){    if (mMultiThread) {        //for now, multi-threaded planners are not allowed to render        //rendering should only be done by the main thread        return;    }    if (mRenderType == RENDER_BEST) {        if (mBestList.empty()) {            return;        }        if (mLastRenderState == mBestList.front()) {            return;        }        mLastRenderState = mBestList.front();        mBestList.front()->execute();    } else if (mRenderType == RENDER_LEGAL) {        if (mRenderCount >= 20) {            DBGP("Render: geom is " << mHand->getRenderGeometry());            mRenderCount = 0;            if (mCurrentState && mCurrentState->isLegal()) {                mCurrentState->execute();            }        } else {            mRenderCount++;        }    } else if (mRenderType == RENDER_ALWAYS) {        mCurrentState->execute();    } else if (mRenderType == RENDER_NEVER) {        return;    }}