planner::Object* CreateObject(const ObjectData& data, const std::vector<Eigen::Vector3d>& dictionaryPoints, const T_PointReplacement &replacement)
{
//create PQP MODEL
PQP_Model* m = new PQP_Model;
std::size_t currentIndex_(0);
m->BeginModel();
for(std::vector<ObjectTri>::const_iterator tit = data.triangles_.begin();
tit != data.triangles_.end(); ++tit)
{
PQP_REAL points [3][3];
for(int i=0; i< 3; ++i)
{
Vector3toArray(FindPointIndex(tit->points[i],dictionaryPoints, replacement),points[i]);
}
m->AddTri(points[0], points[1], points[2], currentIndex_++);
}
m->EndModel();
return new planner::Object(m,data.normals_, data.name);
}
int main(int argc, char **argv)
{
glutInit(&argc, argv);
glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH | GLUT_MULTISAMPLE);
glutInitWindowSize(512, 512);
// create the window
glutCreateWindow("PQP Demo - Spinning");
// set OpenGL graphics state -- material props, perspective, etc.
InitViewerWindow();
// set the callbacks
glutDisplayFunc(DisplayCB);
glutIdleFunc(IdleCB);
glutMouseFunc(MouseCB);
glutMotionFunc(MotionCB);
glutKeyboardFunc(KeyboardCB);
// initialize the bunny
FILE *fp;
int i, ntris;
bunny_to_draw = new Model("bunny.tris");
fp = fopen("bunny.tris","r");
if (fp == NULL) { fprintf(stderr,"Couldn't open bunny.tris\n"); exit(-1); }
fscanf(fp,"%d",&ntris);
bunny.BeginModel();
for (i = 0; i < ntris; i++)
{
double p1x,p1y,p1z,p2x,p2y,p2z,p3x,p3y,p3z;
fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf",
&p1x,&p1y,&p1z,&p2x,&p2y,&p2z,&p3x,&p3y,&p3z);
PQP_REAL p1[3],p2[3],p3[3];
p1[0] = (PQP_REAL)p1x; p1[1] = (PQP_REAL)p1y; p1[2] = (PQP_REAL)p1z;
p2[0] = (PQP_REAL)p2x; p2[1] = (PQP_REAL)p2y; p2[2] = (PQP_REAL)p2z;
p3[0] = (PQP_REAL)p3x; p3[1] = (PQP_REAL)p3y; p3[2] = (PQP_REAL)p3z;
bunny.AddTri(p1,p2,p3,i);
}
bunny.EndModel();
fclose(fp);
// initialize the torus
torus_to_draw = new Model("torus.tris");
fp = fopen("torus.tris","r");
if (fp == NULL) { fprintf(stderr,"Couldn't open torus.tris\n"); exit(-1); }
fscanf(fp,"%d",&ntris);
torus.BeginModel();
for (i = 0; i < ntris; i++)
{
double p1x,p1y,p1z,p2x,p2y,p2z,p3x,p3y,p3z;
fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf",
&p1x,&p1y,&p1z,&p2x,&p2y,&p2z,&p3x,&p3y,&p3z);
PQP_REAL p1[3],p2[3],p3[3];
p1[0] = (PQP_REAL)p1x; p1[1] = (PQP_REAL)p1y; p1[2] = (PQP_REAL)p1z;
p2[0] = (PQP_REAL)p2x; p2[1] = (PQP_REAL)p2y; p2[2] = (PQP_REAL)p2z;
p3[0] = (PQP_REAL)p3x; p3[1] = (PQP_REAL)p3y; p3[2] = (PQP_REAL)p3z;
torus.AddTri(p1,p2,p3,i);
}
torus.EndModel();
fclose(fp);
// print instructions
printf("PQP Demo - Spinning:\n"
"Press 'q' to quit.\n"
"Press any other key to toggle animation.\n"
"Left-drag left & right to change angle of view.\n"
"Left-drag up & down to change elevation of view.\n"
"Right-drag up & down to change distance of view.\n");
// Enter the main loop.
glutMainLoop();
return 0;
}
double ArizonaTest::getScale2Surface(double *dis)
{
if (coords.empty() || indices.empty()) {
DBGA("Unable to compute minimum force; hull not set");
return 0;
}
if(mIsFlipped){
dis[0] = - dis[0];
dis[1] = - dis[1];
dis[2] = - dis[2];
}
// compute the necessary force
PQP_REAL v1[3],v2[3],v3[3];
PQP_Model volume = PQP_Model();
//model for the volumn
volume.BeginModel();
int triInd = 0;
int numIndices = indices.size();
for (int k = 0; k < numIndices - 3; k++) {
if(indices[k+3] == -1) // dealing with meaningful vertices.
{
v1[0]=coords[indices[k]].x();
v1[1]=coords[indices[k]].y();
v1[2]=coords[indices[k]].z();
v2[0]=coords[indices[k+1]].x();
v2[1]=coords[indices[k+1]].y();
v2[2]=coords[indices[k+1]].z();
v3[0]=coords[indices[k+2]].x();
v3[1]=coords[indices[k+2]].y();
v3[2]=coords[indices[k+2]].z();
volume.AddTri(v1,v2,v3,triInd++);
}
}
volume.EndModel();
PQP_REAL pt[3];
pt[0] = (double)(dis[0]*mGrasp->getMaxRadius());
pt[1] = (double)(dis[1]*mGrasp->getMaxRadius());
pt[2] = (double)(dis[2]*mGrasp->getMaxRadius());
vec3 disturbance(pt[0], pt[1], pt[2]);
double originalLength = disturbance.len();
double startValue = 0, endValue = originalLength;
int loops = 0;
while (!isOutside(&volume, pt) ) {
pt[0] *= 2; pt[1] *= 2; pt[2] *= 2;
startValue = endValue;
endValue *= 2;
loops ++;
if (loops > 20) {
DBGA("Failed to find point outside of volume!!!!!");
break;
}
}
disturbance = normalise(disturbance);
double length = binarySearch(disturbance, volume, startValue, endValue);
return originalLength / length;
}