Menu
▾
▴
[Freesolid-cvs] freesolid/libsolid C-api.cpp,1.11,1.12 Convex.cpp,1.7,1.8 Polyhedron.cpp,1.5,1.6
|
From: Rodrigo H. <kw...@us...> - 2005-07-07 23:20:10
|
Update of /cvsroot/freesolid/freesolid/libsolid In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5045/libsolid Modified Files: C-api.cpp Convex.cpp Polyhedron.cpp Log Message: more penetration depth code Index: C-api.cpp =================================================================== RCS file: /cvsroot/freesolid/freesolid/libsolid/C-api.cpp,v retrieving revision 1.11 retrieving revision 1.12 diff -C2 -d -r1.11 -r1.12 *** C-api.cpp 6 Jul 2005 23:40:57 -0000 1.11 --- C-api.cpp 7 Jul 2005 23:19:54 -0000 1.12 *************** *** 361,371 **** break; case DT_SMART_RESPONSE: ! fprintf(stdout,"object_test DT_SMART_RESPONSE\n"); if (prev_closest_points(*e.obj1, *e.obj2, e.sep_axis, p1, p2)) { ! fprintf(stdout,"prev_closest_points returned true\n"); ! fprintf(stdout,"Previous Points:\n%f,%f,%f\n%f,%f,%f\n", ! e.obj1->prev(p1)[X],e.obj1->prev(p1)[Y],e.obj1->prev(p1)[Z], ! e.obj2->prev(p2)[X],e.obj2->prev(p2)[Y],e.obj1->prev(p2)[Z]); Vector v = e.obj1->prev(p1) - e.obj2->prev(p2); resp(e.obj1->ref, e.obj2->ref, p1, p2, v); --- 361,371 ---- break; case DT_SMART_RESPONSE: ! //fprintf(stdout,"object_test DT_SMART_RESPONSE\n"); if (prev_closest_points(*e.obj1, *e.obj2, e.sep_axis, p1, p2)) { ! // fprintf(stdout,"prev_closest_points returned true\n"); ! // fprintf(stdout,"Previous Points:\n%f,%f,%f\n%f,%f,%f\n", ! // e.obj1->prev(p1)[X],e.obj1->prev(p1)[Y],e.obj1->prev(p1)[Z], ! // e.obj2->prev(p2)[X],e.obj2->prev(p2)[Y],e.obj1->prev(p2)[Z]); Vector v = e.obj1->prev(p1) - e.obj2->prev(p2); resp(e.obj1->ref, e.obj2->ref, p1, p2, v); Index: Polyhedron.cpp =================================================================== RCS file: /cvsroot/freesolid/freesolid/libsolid/Polyhedron.cpp,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** Polyhedron.cpp 21 Apr 2005 23:20:55 -0000 1.5 --- Polyhedron.cpp 7 Jul 2005 23:19:54 -0000 1.6 *************** *** 1,4 **** /* ! SOLID - Software Library for Interference Detection Copyright (C) 1997-1998 Gino van den Bergen --- 1,4 ---- /* ! FreeSOLID - Software Library for Interference Detection Copyright (C) 1997-1998 Gino van den Bergen *************** *** 43,47 **** Polyhedron::Polyhedron(const VertexBase& b, int c, const unsigned int v[]) : ! Polytope(b, c, v), cobound(0) { boolT ismalloc; int curlong, totlong, exitcode; --- 43,48 ---- Polyhedron::Polyhedron(const VertexBase& b, int c, const unsigned int v[]) : ! Polytope(b, c, v), cobound(0) ! { boolT ismalloc; int curlong, totlong, exitcode; *************** *** 55,65 **** coordT *p = &array[0]; int i; ! for (i = 0; i < numVerts(); ++i) { ! *p++ = (*this)[i][X]; ! *p++ = (*this)[i][Y]; ! *p++ = (*this)[i][Z]; ! } ! ismalloc = False; // True if qh_freeqhull should 'free(array)' qh_init_A(stdin, stdout, stderr, 0, NULL); if (exitcode = setjmp(qh errexit)) exit(exitcode); --- 56,67 ---- coordT *p = &array[0]; int i; ! for (i = 0; i < numVerts(); ++i) ! { ! *p++ = (*this)[i][X]; ! *p++ = (*this)[i][Y]; ! *p++ = (*this)[i][Z]; ! } ! ismalloc = False; // True if qh_freeqhull should 'free(array)' qh_init_A(stdin, stdout, stderr, 0, NULL); if (exitcode = setjmp(qh errexit)) exit(exitcode); *************** *** 72,86 **** IndexBuf* indexBuf = new IndexBuf[numVerts()]; IndexBuf facetIndices; ! FORALLfacets { ! setT *vertices = qh_facet3vertex(facet); ! FOREACHvertex_(vertices) facetIndices.push_back(qh_pointid(vertex->point)); ! size_t facetIndiceCount = facetIndices.size(); ! for (size_t i = 0, j = facetIndiceCount-1; ! i < facetIndiceCount; j = i++) { ! indexBuf[facetIndices[j]].push_back(facetIndices[i]); } ! facetIndices.erase(facetIndices.begin(), facetIndices.end()); ! } cobound = new IndexArray[numVerts()]; --- 74,90 ---- IndexBuf* indexBuf = new IndexBuf[numVerts()]; IndexBuf facetIndices; ! FORALLfacets ! { ! setT *vertices = qh_facet3vertex(facet); ! FOREACHvertex_(vertices) ! facetIndices.push_back(qh_pointid(vertex->point)); ! size_t facetIndiceCount = facetIndices.size(); ! for (size_t i = 0, j = facetIndiceCount-1; ! i < facetIndiceCount; j = i++) { ! indexBuf[facetIndices[j]].push_back(facetIndices[i]); } ! facetIndices.erase(facetIndices.begin(), facetIndices.end()); ! } cobound = new IndexArray[numVerts()]; *************** *** 91,98 **** curr_vertex = 0; while (indexBuf[curr_vertex].size() == 0) ++curr_vertex; ! delete [] indexBuf; delete [] array; ! qh NOerrexit = True; qh_freeqhull(!qh_ALL); --- 95,102 ---- curr_vertex = 0; while (indexBuf[curr_vertex].size() == 0) ++curr_vertex; ! delete [] indexBuf; delete [] array; ! qh NOerrexit = True; qh_freeqhull(!qh_ALL); *************** *** 100,104 **** } ! Polyhedron::~Polyhedron() { delete [] cobound; } --- 104,109 ---- } ! Polyhedron::~Polyhedron() ! { delete [] cobound; } *************** *** 128,137 **** Polyhedron::~Polyhedron() {} ! Point Polyhedron::support(const Vector& v) const { int c = 0; Scalar h = dot((*this)[0], v), d; ! for (int i = 1; i < numVerts(); ++i) { ! if ((d = dot((*this)[i], v)) > h) { c = i; h = d; } ! } return (*this)[c]; } --- 133,144 ---- Polyhedron::~Polyhedron() {} ! Point Polyhedron::support(const Vector& v) const ! { int c = 0; Scalar h = dot((*this)[0], v), d; ! for (int i = 1; i < numVerts(); ++i) ! { ! if ((d = dot((*this)[i], v)) > h) { c = i; h = d; } ! } return (*this)[c]; } Index: Convex.cpp =================================================================== RCS file: /cvsroot/freesolid/freesolid/libsolid/Convex.cpp,v retrieving revision 1.7 retrieving revision 1.8 diff -C2 -d -r1.7 -r1.8 *** Convex.cpp 6 Jul 2005 23:40:57 -0000 1.7 --- Convex.cpp 7 Jul 2005 23:19:54 -0000 1.8 *************** *** 24,27 **** --- 24,39 ---- P.O. Box 513, 5600 MB Eindhoven, The Netherlands */ + + #ifdef HAVE_CONFIG_H + #include <config.h> + #endif + + #ifdef USE_QHULL + extern "C" + { + #include "qhull_a.h" + } + #endif + #include <vector> #include "Convex.h" *************** *** 239,242 **** --- 251,499 ---- } + + void closest_point_in_triangle(const Vector& p, + const Vector& a, + const Vector& b, + const Vector& c, + Vector& out) + { + // Check if P in vertex region outside A + Vector ab = b - a; + Vector ac = b - a; + Vector ap = p - a; + float d1 = dot(ab,ap); + float d2 = dot(ac,ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + out = a; // barycentric coordinates (1,0,0) + //fprintf(stdout,"1\n"); + return; + } + + // Check if P in vertex region outside B + Vector bp = p - b; + float d3 = dot(ab,bp); + float d4 = dot(ac,bp); + if (d3 >= 0.0f && d4 <= d3) + { + out = b; + return; // barycentric coordinates (0,1,0) + } + + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = (d1*d4) - (d3*d2); + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + { + float v = d1 / (d1 - d3); + out = a + (ab*v); // barycentric coordinates (1-v,v,0) + return; + } + + // Check if P in vertex region outside C + Vector cp = p - b; + float d5 = dot(ab,cp); + float d6 = dot(ac,cp); + if (d6 >= 0.0f && d5 <= d6) + { + out=b; + return; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = (d5*d2) - (d1*d6); + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) + { + float w = d2 / (d2 - d6); + out = a + (ac*w); // barycentric coordinates (1-w,0,w) + return; + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = (d3*d6) - (d5*d4); + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) + { + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + out= b + ((b - b)*w); + // barycentric coordinates (0,1-w,w) + return; + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + out = a + (ab * v) + (ac * w); + } + + void closest_point_in_line(const Vector& p, + const Vector& a, + const Vector& b, + Vector& out) + { + // Create the vector from end point a to our point p. + Vector vVector1 = p - a; + // Create a normalized direction vector from end point a to end point b + Vector vVector2 = b - a; + vVector2.normalize(); + // Use the distance formula to find the distance of + // the line segment (or magnitude) + float d = sqrtf((vVector2[X]*vVector2[X])+ + (vVector2[Y]*vVector2[Y])+ + (vVector2[Z]*vVector2[Z])); + + // Using the dot product, we project the vVector1 onto the vector vVector2. + // This essentially gives us the distance from our projected vector from a. + float t = dot(vVector2,vVector1); + // If our projected distance from a, "t", is less than or equal to 0, + // it must be closest to the end point a. We want to return this end point. + if (t <= 0) + { + out = a; + return; + } + + // If our projected distance from a, "t", + // is greater than or equal to the magnitude + // or distance of the line segment, + // it must be closest to the end point b. So, return b. + if (t >= d) + { + out = b; + return; + } + + // Here we create a vector that is of length t + // and in the direction of vVector2 + Vector vVector3 = vVector2 * t; + + // To find the closest point on the line segment, + // we just add vVector3 to the original + // end point a. + Vector vClosestPoint = a + vVector3; + // Return the closest point on the line segment + out = vClosestPoint; + } + + /*! + \brief Determines minimum penetration depth vector for current simplex + \param [IN] a Convex Shape A + \param [IN] b Convex Shape B + \param [IN] a2w A transformation + \param [IN] b2w B transformation + \param [OUT] v penetration depth vector + */ + inline void determine_minimum_penetration_depth(const Convex& a, + const Convex& b, + const Transform& a2w, + const Transform& b2w, + Vector& v) + { + #ifdef USE_QHULL + /* + This is a very RAW implementation of the Penetration Depth calculation + described on the paper + "Proximity Queries and Penetration Depth Computation on 3D Game Objects" + found at http://www.win.tue.nl/~gino/solid/gdc2001depth.pdf + */ + boolT ismalloc; + int curlong, totlong, exitcode; + char options[200]; + + facetT *facet; + vertexT *vertex; + vertexT **vertexp; + const Vector origin(0,0,0); + Vector closest; + Vector close_test; + Vector w; + boolT isoutside; + realT bestdist; + int triangle_index[3]; + Vector triangle[3]; + int iterations=0; + // LOOP Starts Here, testing with 100 iterations: + for(int j=0;j<100;++j) + { + size_t numVerts = inflated_polyhedron.size(); + + coordT *array = new coordT[numVerts*3]; + coordT *p = &array[0]; + int i; + for (i = 0; i < numVerts; ++i) + { + *p++ = inflated_polyhedron[i][X]; + *p++ = inflated_polyhedron[i][Y]; + *p++ = inflated_polyhedron[i][Z]; + } + + ismalloc = False; // True if qh_freeqhull should 'free(array)' + qh_init_A(stdin, stdout, stderr, 0, NULL); + if (exitcode = setjmp(qh errexit)) exit(exitcode); + sprintf(options, "qhull Qx i s Tcv C-0"); + qh_initflags(options); + qh_init_B(&array[0], numVerts, 3, ismalloc); + qh_qhull(); + qh_check_output(); + + Scalar distance=FLT_MAX; + + FORALLfacets + { + setT *vertices = qh_facet3vertex(facet); + i=0; + FOREACHvertex_(vertices) + { + triangle_index[i]=qh_pointid(vertex->point); + ++i; + } + closest_point_in_triangle(origin, + inflated_polyhedron[triangle_index[0]], + inflated_polyhedron[triangle_index[1]], + inflated_polyhedron[triangle_index[2]], + close_test); + if(close_test.length2()<distance) + { + triangle[0]=inflated_polyhedron[triangle_index[0]]; + triangle[1]=inflated_polyhedron[triangle_index[1]]; + triangle[2]=inflated_polyhedron[triangle_index[2]]; + distance=close_test.length2(); + closest=close_test; + } + } + + // Split triangle, add points to the convex hull + /* + Removing points NOT in the convex hull here from inflated_polyhedron + would probably be a good idea. + */ + w = a2w(a.support((-closest) * a2w.getBasis())) - + b2w(b.support(closest * b2w.getBasis())); + inflated_polyhedron.push_back(w); + + for(i=0;i<3;++i) + { + // Use closest point in line to find new w + closest_point_in_line(origin,triangle[i],triangle[(i+1)%3],closest); + w = a2w(a.support((-closest) * a2w.getBasis())) - + b2w(b.support(closest * b2w.getBasis())); + if(dot(w,closest)!=dot(closest,closest)) + { + inflated_polyhedron.push_back(w); + } + } + + delete [] array; + qh NOerrexit = True; + qh_freeqhull(!qh_ALL); + qh_memfreeshort(&curlong, &totlong); + } + v=w; + fprintf(stdout,"Last V %f,%f,%f\n",v[X],v[Y],v[Z]); + + #else + #warning "Determination of minimum penetration depth without QHULL has not been implemented" + #endif + } + /*! \brief Checks whether or not convex shapes A and B intersect *************** *** 298,306 **** case 0: /* ! Simplex is a point this should not happen, ! maybe if the 2 objects are duplicates and ! concentric, for now, just report. */ ! fprintf(stdout,"Panic: Simplex is a Point!\n"); break; case 1: --- 555,563 ---- case 0: /* ! Simplex is a point this happens if the 2 surfaces barelly touch */ ! v[X]=0.0f; ! v[Y]=0.0f; ! v[Z]=0.0f; break; case 1: *************** *** 334,339 **** inflated_polyhedron.push_back(y[i]); } ! // ??????? ! // Profit! :) break; } --- 591,595 ---- inflated_polyhedron.push_back(y[i]); } ! determine_minimum_penetration_depth(a,b,a2w,b2w,v); break; } *************** *** 524,531 **** } - /*! \brief returns the shortest penetration depth for the last test */ - void shortest_penetration_depth(Vector& pd) - { - } - - --- 780,781 ---- |
