void Physics::TestCollision(Circle *a, Polygon *b) {
Vector2D n, p;
Line edge;
double minDepth = DBL_INF;
for (int i = 0; i < b->nPoints; i++) {
double min0, max0, min1, max1;
Vector2D normal = b->GetNormal(i);
b->GetProjection(normal, min0, max0);
a->GetProjection(normal, min1, max1);
if (max0 < min1 || max1 < min0) return;
Line tempEdge(b->GetPoint(i), b->GetPoint((i + 1) % b->nPoints));
Vector2D tempP = tempEdge.a + (a->GetCentroidPosition() - tempEdge.a) * tempEdge.v.GetDirection() * tempEdge.v.GetDirection();
if (tempEdge.IsInside(tempP) && max0 < max1 && min1 < max0 && max0 - min1 < minDepth) {
minDepth = max0 - min1;
n = normal;
edge = Line(b->GetPoint(i), b->GetPoint((i + 1) % b->nPoints));
p = tempP;
}
}
if (minDepth < DBL_INF)
constraints.push_back(new Contact(b, a, p, n, minDepth));
for (int i = 0; i < b->nPoints; i++) {
Vector2D p = b->GetTransformToWorld()(b->points[i]);
if (a->IsPointInside(p)) {
double depth = a->radius - (a->GetCentroidPosition() - p).GetLength();
constraints.push_back(new Contact(a, b, p, (p - a->GetCentroidPosition()).GetDirection(), depth));
return;
}
}
}
TriMesh::TriMesh(const std::vector<Vec3f>& x,
const std::vector<Vec3i>& tri)
: vertices(x.size()), faces(tri.size()), edges(tri.size()*3)
{
for (size_t v = 0; v < vertices.size(); ++v)
{
vertices[v].p = x[v];
}
// For keeping track of edges that we haven't paired off yet.
std::map<std::pair<int,int>,int> unmatchedEdges;
std::map<std::pair<int,int>,int>::iterator edgeItr;
for (size_t f = 0; f < faces.size(); ++f)
{
const Vec3i& t = tri[f];
HalfEdge* e = &(edges[3*f]);
for (int i = 0; i < 3; ++i)
{
// Associate vertex and edge.
e[i].v = &(vertices[t[i]]);
e[i].v->e = &(e[i]);
// Associate edge with face.
e[i].f = &(faces[f]);
// Link up edges.
e[i].prev = &(e[(i+2)%3]);
e[i].next = &(e[(i+1)%3]);
// Match symmetric edges
std::pair<int,int> tempEdge(t[i], t[(i+1)%3]);
if (tempEdge.first > tempEdge.second)
{
// Lower vertex index always comes first.
tempEdge.first = tempEdge.second;
tempEdge.second = t[i];
}
edgeItr = unmatchedEdges.find(tempEdge);
if (edgeItr == unmatchedEdges.end())
{
// The pair to this edge has not been found yet.
// Save the index of the current half-edge for later.
unmatchedEdges[tempEdge] = 3*f+i;
}
else
{
// The pair was already found, pair it with the current edge.
e[i].sym = &(edges[edgeItr->second]);
e[i].sym->sym = &(e[i]);
unmatchedEdges.erase(edgeItr);
}
}
// Associate face with the first edge.
e[0].f->e = &(e[0]);
}
}
/** Generates the specified no of edges
Selects a source and destination node randomly using rand function **/
void Graph::generateEdges()
{
unsigned int i;
edgeItr eItr,NewEnd;
edgeList.clear();
Edge *e;
//Generate one edge for each vertex in the graph
for(i=0;i<noVertices;i++)
{
e = new Edge(i+1,randomNodeID(),randomEdgeWeight());
//Re-generate until source and destination nodes are different
while(e->getDst() == e->getSrc())
e->setDst(randomNodeID());
edgeList.push_back(*e);
edgeList.push_back(*e);
edgeList.back().swap();
delete e;
}
//Generate the remaining no. of edges
for(i=0;i<(noEdges-noVertices);i++)
{
Edge tempEdge(randomNodeID(),randomNodeID(),randomEdgeWeight());
while(tempEdge.getDst() == tempEdge.getSrc())
tempEdge.setDst(randomNodeID());
edgeList.push_back(tempEdge);
edgeList.push_back(tempEdge);
edgeList.back().swap();
}
stxxl::sort(edgeList.begin(),edgeList.end(),myCmpSrc(),INTERNAL_MEMORY_FOR_SORTING);
//Remove duplicate edges
NewEnd = edgeList.begin() ;
for(eItr=edgeList.begin()+1; eItr!= edgeList.end(); eItr++)
{
if(!(*NewEnd == *eItr))
{
NewEnd++;
*NewEnd = *eItr;
}
}
NewEnd++;
edgeList.resize(NewEnd - edgeList.begin());
//Store edges sorted the key <Source vertex, Edge weight>
stxxl::sort(edgeList.begin(),edgeList.end(),myCmpEdgeWt(),INTERNAL_MEMORY_FOR_SORTING);
noEdges = edgeList.size()/2;
STXXL_MSG("Edge vector created of size: "<<edgeList.size());
}
/** Generates a complete graph **/
void Graph::generateCompleteGraph()
{
unsigned int weight;
edgeItr eItr,NewEnd;
noEdges = noVertices * (noVertices - 1) / 2;
edgeList.clear();
//Generate noVertices * (noVertices - 1) / 2 number of edges with edge weights genrated randomly
for (unsigned int i=0; i<noVertices; i++)
{
for (unsigned int j=0; j<i; j++)
{
weight = randomEdgeWeight();
Edge tempEdge(i+1,j+1,weight);
edgeList.push_back(tempEdge);
edgeList.push_back(tempEdge);
edgeList.back().swap();
}
}
stxxl::sort(edgeList.begin(),edgeList.end(),myCmpSrc(),INTERNAL_MEMORY_FOR_SORTING);
NewEnd = edgeList.begin() ;
for(eItr=edgeList.begin()+1; eItr!= edgeList.end(); eItr++)
{
if(!(NewEnd == eItr))
{
NewEnd++;
*NewEnd = *eItr;
}
}
NewEnd++;
edgeList.resize(NewEnd - edgeList.begin());
stxxl::sort(edgeList.begin(),edgeList.end(),myCmpEdgeWt(),INTERNAL_MEMORY_FOR_SORTING);
noEdges = edgeList.size()/2;
STXXL_MSG("Edge vector created of size: "<<edgeList.size());
//Generate the vertex list
generateVertexList();
}
