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;*/
}
}
