std::vector<PQP_Model *> Convexer::fromaiScene(const aiScene *scene) {
// Get the triangles
std::vector<PQP_Model *> ret_val;
PQP_REAL v1[3], v2[3], v3[3], v4[3];
for (unsigned int i = 0; i < scene->mNumMeshes; i++) {
aiMesh *curr_mesh = scene->mMeshes[i];
if (curr_mesh != NULL) {
PQP_Model *mod = new PQP_Model();
for (unsigned int k = 0; k < curr_mesh->mNumFaces; k++) {
aiFace &curr_face = curr_mesh->mFaces[k];
if (curr_face.mNumIndices == 3) {
// Triangualr faces!! --> ok (TODO: triangulate the non-triangular faces
v1[0] = curr_mesh->mVertices[curr_face.mIndices[0]].x;
v1[1] = curr_mesh->mVertices[curr_face.mIndices[0]].y;
v1[2] = curr_mesh->mVertices[curr_face.mIndices[0]].z;
v2[0] = curr_mesh->mVertices[curr_face.mIndices[1]].x;
v2[1] = curr_mesh->mVertices[curr_face.mIndices[1]].y;
v2[2] = curr_mesh->mVertices[curr_face.mIndices[1]].z;
v3[0] = curr_mesh->mVertices[curr_face.mIndices[2]].x;
v3[1] = curr_mesh->mVertices[curr_face.mIndices[2]].y;
v3[2] = curr_mesh->mVertices[curr_face.mIndices[2]].z;
// Debug
// std::cout << "Adding triangle: (" << v1[0] << ", " << v1[1] << ", " << v1[2] << ") ";
// std::cout << "(" << v2[0] << ", " << v2[1] << ", " << v2[2] << ") ";
// std::cout << "(" << v3[0] << ", " << v3[1] << ", " << v3[2] << ")" << std::endl;
mod->AddTri(v1, v2, v3, k);
} else if (curr_face.mNumIndices == 4) {
v1[0] = curr_mesh->mVertices[curr_face.mIndices[0]].x;
v1[1] = curr_mesh->mVertices[curr_face.mIndices[0]].y;
v1[2] = curr_mesh->mVertices[curr_face.mIndices[0]].z;
v2[0] = curr_mesh->mVertices[curr_face.mIndices[1]].x;
v2[1] = curr_mesh->mVertices[curr_face.mIndices[1]].y;
v2[2] = curr_mesh->mVertices[curr_face.mIndices[1]].z;
v3[0] = curr_mesh->mVertices[curr_face.mIndices[2]].x;
v3[1] = curr_mesh->mVertices[curr_face.mIndices[2]].y;
v3[2] = curr_mesh->mVertices[curr_face.mIndices[2]].z;
v4[0] = curr_mesh->mVertices[curr_face.mIndices[3]].x;
v4[1] = curr_mesh->mVertices[curr_face.mIndices[3]].y;
v4[2] = curr_mesh->mVertices[curr_face.mIndices[3]].z;
mod->AddTri(v1, v2, v3, k);
mod->AddTri(v1, v4, v3, k);
// std::cout << "Adding quadrangle: (" << v1[0] << ", " << v1[1] << ", " << v1[2] << ") ";
// std::cout << "(" << v2[0] << ", " << v2[1] << ", " << v2[2] << ") ";
// std::cout << "(" << v3[0] << ", " << v3[1] << ", " << v3[2] << ")" << std::endl;
} else {
std::cerr << "Warning faces with more than 4 edges are not allowed at this moment.\n";
}
}
if (curr_mesh->mNumFaces > 0 && mod->num_tris > 0) {
mod->EndModel();
ret_val.push_back(mod);
}
}
}
return ret_val;
}
void Convexer::addModels(HACD::HACD *myHACD, std::vector<PQP_Model *> &mod_vec){
PQP_REAL v[3][3];
cout << "addModels --> Num clusters: " << myHACD->GetNClusters() << endl;
myHACD->Save("/home/sinosuke/out.wrl", false);
for(size_t c = 0; c < myHACD->GetNClusters(); ++c) {
cout << "addModels --> cluster " << c << endl;
PQP_Model *mod = new PQP_Model();
unsigned int id = 0;
// Get the cluster
size_t nPoints = myHACD->GetNPointsCH(c);
size_t nTriangles = myHACD->GetNTrianglesCH(c);
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD->GetCH(c, pointsCH, trianglesCH);
for (unsigned int i = 0; i < nTriangles; i++) {
// cout << "addModels --> Adding triangle: " ;
//
// for (int j = 0; j < 3; j++) {
// cout << "(";
// for (int k = 0; k < 3; k++) {
// v[j][k] = pointsCH[trianglesCH[i][j]][k];
// cout << v[j][k] << " ";
// }
// cout << ") ";
// }
mod->AddTri(v[0], v[1], v[2], id);
id++;
}
cout <<endl;
mod->EndModel();
mod_vec.push_back(mod);
delete[] pointsCH;
delete[] trianglesCH;
}
}
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;
}
