void CollisionRespone::run(){ unsigned int i; findNeighbour(); std::list<compositePhysobj *>::iterator it; for(it=parent->objects.begin();it!=parent->objects.end();it++){ if((*it)->hasRigid){ for(i=0;i<(*it)->CollsionContacts.size();i++){ DataCarrier=(*it)->CollsionContacts[i]; ImpulseResponse(DataCarrier.obj1,DataCarrier.obj2); Friction(DataCarrier.obj1,DataCarrier.obj2); } } } /* for(i=0;i<collisionGroups.size();i++){ ActiveCollisionList=collisionGroups[i]; ContactForceResponse(); } */ }
void Planner::updateVertex(list<Uelem> &U, Grid &u, vector<vector<int> > &g, vector<vector<int> > &rhs, Map &map,int km) { // update the vertex value and update the priority queue if(u!=map.goal) { vector<Grid> neighbour = findNeighbour(map, u, 8); vector<float> dist; for_each(neighbour.begin(),neighbour.end(),[&](Grid &it){dist.push_back((float)g[it.y][it.x]+calculateDistance(it,u));}); rhs[u.y][u.x]=*min_element(dist.begin(),dist.end()); } auto pt =find_if(U.begin(),U.end(),[u](Uelem &pt){return pt.point==u;}); if (pt!=U.end()) U.erase(pt); if (g[u.y][u.x]!=rhs[u.y][u.x]) { Key knew = calculateKey(u,g,rhs,map.start,0); auto pt = find_if(U.begin(),U.end(),[&](Uelem &a){return a.key<knew;}); U.insert(pt,{u,knew}); } }
int findNeighbour(char *s,int x,int y) { int n; if(*s=='\0') return 1; if(isSafe(x,y,s)) { vis[x][y]=1; for(n=0;n<8;n++) { if(findNeighbour(s+1,x+r[n],y+c[n])) return 1; } vis[x][y]=0; return 0; } return 0; }
void find(char *s) { int i,j; for(i=0;i<M;i++) { for(j=0;j<N;j++) { if(P[i][j]==*s) { if(findNeighbour(s,i,j)) { printf("YES\n"); return; } } } } printf("NO\n"); }
vector<Grid> Planner::dStarLite(Map &map) { // this is the implementation of the D star lite algorithm vector<Grid> wayPoint; // Initialization vector<vector<int> > g(map._rows,vector<int>(map._cols,map._rows*map._cols+1)); vector<vector<int> > rhs(map._rows,vector<int>(map._cols,map._rows*map._cols+1)); km =0; rhs[map.goal.y][map.goal.x] =0; Key n = calculateKey(map.goal,g,rhs,map.start,0); U.push_back({map.goal,n}); // Computer current shortest path // Update the states of the map if the first element in the priority list can be updated or the goal is not reached while(U.front().key<calculateKey(map.goal,g,rhs,map.start,0)||(rhs[map.start.y][map.start.x]!=g[map.start.y][map.start.x])) { // take the key value and postion of the first element in the priority queue Key kold = U.front().key; Grid u = U.front().point; U.pop_front(); Key knew = calculateKey(u,g,rhs,map.start,km); // calculate the new key value // update map if old value is different from the new value if (kold<knew) { // if the new key is larger, the cost of the edge of the grid might be change // the current grid should be updated and re-expanded // insert it in the priority queue auto pt =find_if(U.begin(),U.end(),[knew](Uelem &u){return u.key<knew;}); U.insert(pt,{u,knew}); } else if (g[u.y][u.x]>rhs[u.y][u.x]) { // if the grid is overconstraint, there are new shorter paths detected g[u.y][u.x] = rhs[u.y][u.x]; // update all its neighbour value vector<Grid> neightbour = findNeighbour(map, u, 8); for(auto &n:neightbour) { updateVertex(U,n,g,rhs,map,km); } } else { // if the grid is underconstraint, the grid it self and its neightbour should all be updated g[u.y][u.x] = map._cols*map._rows; vector<Grid> neightbour = findNeighbour(map, u, 8); for(auto &n:neightbour) { updateVertex(U,n,g,rhs,map,km); } updateVertex(U,u,g,rhs,map,km); } } return wayPoint; }
vector<Grid> Planner::aStarPlanning(Map &map) { // implemtation of the a star algorithm // initialize the way point list vector<Grid> wayPoint; // return an empty list if map is invalid if(map.start.x==-1||map.start.y==-1||map.goal.x==-1||map.goal.y==-1) return wayPoint; // type of the A star priority list element typedef struct {Grid pos; float f;} AStarUnit; list<AStarUnit> openlist; // open list vector<vector<float> > h(map._rows, vector<float>(map._cols, -1.0)); // heuristic value vector<vector<float> > g(map._rows, vector<float>(map._cols, -1.0)); // optimized value of the grid vector<vector<float> > f(map._rows, vector<float>(map._cols, -1.0)); // the key value of the A star algorithm (search priority) vector<vector<Grid> > b(map._rows, vector<Grid>(map._cols, Grid(-1,-1))); // back pointer to pred node vector<vector<bool> > closelist(map._rows, vector<bool>(map._cols, false)); // close list // value initialization h[map.start.y][map.start.x] = calculateDistance(map.start,map.goal); g[map.start.y][map.start.x] = 0; f[map.start.y][map.start.x] = h[map.start.y][map.start.x]; AStarUnit startP={map.start,calculateDistance(map.start,map.goal)}; openlist.push_back(startP); while (openlist.size()>0) { // iterate when the open list is not empty // pop the first item in the open list to expand Grid currentP = openlist.front().pos; closelist[currentP.y][currentP.x] = true; openlist.pop_front(); // if already reach the goal, stop iterating if (currentP==map.goal) break; // find its direct neigbour vector<Grid> neighbourList = findNeighbour(map,currentP,4); for(Grid neighbour:neighbourList) { // update the shorest distance of the cells neighbour if a shorter path is finded if (((g[neighbour.y][neighbour.x]==-1.0)||(g[neighbour.y][neighbour.x]>g[currentP.y][currentP.x]+calculateDistance(neighbour,currentP)))&&(!closelist[neighbour.y][neighbour.x])) { h[neighbour.y][neighbour.x] = calculateDistance(neighbour,map.goal); g[neighbour.y][neighbour.x] = g[currentP.y][currentP.x]+calculateDistance(neighbour,currentP); f[neighbour.y][neighbour.x] = h[map.start.y][map.start.x]+g[map.start.y][map.start.x]; b[neighbour.y][neighbour.x] = currentP; float _f =f[map.start.y][map.start.x]; // find the correct position to insert the cell into the priority list, according to f value auto insert_pos = find_if(openlist.begin(),openlist.end(),[_f](AStarUnit list_elem){return _f<list_elem.f;}); openlist.insert(insert_pos,{neighbour,_f}); } } } // form the waypoint list from the goal cell in the map deque<Grid> tempWayPoint; Grid trace = map.goal; while (trace!=Grid(-1.0,-1.0)) { tempWayPoint.push_front(trace); trace = b[trace.y][trace.x]; } wayPoint.assign(tempWayPoint.cbegin(),tempWayPoint.cend()); return wayPoint; }
bool insideSurfaceClosest(const Point3D &pTest, const Surface &s, const SpacialHash &faceHash, ClosestPointInfo *inf, float stopBelow, bool allowQuickTest){ if (inf) inf->type = DIST_TYPE_INVALID; // quick bounding box test if (allowQuickTest){ if (pTest.x < s.pMin.x || pTest.x > s.pMax.x || pTest.y < s.pMin.y || pTest.y > s.pMax.y || pTest.z < s.pMin.z || pTest.z > s.pMax.z){ return false; } } ClosestPointInfo localClosestInf; if (!inf) inf = &localClosestInf; float dist = getClosestPoint(inf, pTest, s, faceHash, stopBelow); if (dist < stopBelow){ // optimise for dodec return true; } // vector to point on surface Vector3D v; v.difference(pTest, inf->pClose); v.norm(); if (inf->type == FACE){ // face test Vector3D n; s.getTriangleNormal(&n, inf->triangle); double dt = n.dot(v); return dt <= 0; } else if (inf->type == EDGE){ // edge test const Surface::Triangle *tri = &s.triangles.index(inf->triangle); // edge will be between vertices v[num] and v[(num+1)%3] int e[2]; e[0] = tri->v[inf->num]; e[1] = tri->v[(inf->num+1)%3]; int neigh = findNeighbour(s, *tri, e); if (neigh >= 0){ // make a plane for one of the triangles Vector3D n1; s.getTriangleNormal(&n1, inf->triangle); Point3D p1 = s.vertices.index(e[0]).p; Plane pl1; pl1.assign(n1, p1); // get the point from the other triangle which is not part of edge const Surface::Triangle *tri2 = &s.triangles.index(neigh); for (int i = 0; i < 3; i++){ if (tri2->v[i] != e[0] && tri2->v[i] != e[1]) break; } CHECK_DEBUG0(i != 3); Point3D p2 = s.vertices.index(e[1]).p; // get signed distance to plane float dist = pl1.dist(p2); // need normal for second triangle Vector3D n2; s.getTriangleNormal(&n2, neigh); if (dist <= 0.0f){ // faces form convex spike, back facing to both return v.dot(n1) <= 0 && v.dot(n2) <= 0; } else{ // faces form concavity, back facing to either return v.dot(n1) <= 0 || v.dot(n2) <= 0; } } else{ OUTPUTINFO("HHHHHMMMMMMM loose edge\n"); return false; // only one triangle on edge - use face ?? } } else{// if (minType == VERTEX) // chosen triangle const Surface::Triangle *tri = &s.triangles.index(inf->triangle); // chosen vertex int vI = tri->v[inf->num]; Vector3D n; s.getVertexNormal(&n, vI); return n.dot(v) <= 0; /* // get all faces Array<int> tris; s.findNeighbours(&tris, vI, inf->triangle); // behind test for all faces int numTri = tris.getSize(); for (int i = 0; i < numTri; i++){ Vector3D n; s.getTriangleNormal(&n, tris.index(i)); double dt = n.dot(v); if (dt > 0) return false; } // must be behind all return true;*/ } }