void BroadPhaseStrategyA::checkForCollisions(std::vector<Contact>& c)
{
//let us access all the RigidBodys in the Simulation and check if they are in contact broadly via the boundingRadius of their Shape :
//std::vector<std::unique_ptr<ISimulationObject> >::iterator it = sim->simulatedObjects.begin();
#define simul sim->simulatedObjects
for(int i=0;i<simul.size();i++)
{
if( simul[i]->getType() == TSORigidBody)
{
if( ((RigidBody*)(simul[i].get()))->getCollisionStatus() )
{
//let us access to all of the RigidBody that haven't already been checked with regards to this one :
//std::vector<std::unique_ptr<ISimulationObject> >::iterator other = it;
//other++;
ShapeType typeIt = ((RigidBody*)(simul[i].get()))->getShapeType();
for(int o=i+1;o<simul.size();o++)//;other!=sim->simulatedObjects.end();other++)
{
ShapeType typeOther = ((RigidBody*)(simul[o].get()))->getShapeType();
//------------------
if( (simul[o].get())->getType() == TSORigidBody)
{
if( ((RigidBody*)(simul[o].get()))->getCollisionStatus() )
{
Mat<float> midline( ((RigidBody*)(simul[i].get()))->getPosition()-((RigidBody*)(simul[o].get()))->getPosition());
float magnitude = norme2(midline);
if(magnitude < ((RigidBody*)(simul[i].get()))->getShapeReference().getBRadius() + ((RigidBody*)(simul[o].get()))->getShapeReference().getBRadius() )
{
// then there is a potentiel contact :
Contact contact( ((RigidBody*)(simul[i].get())), ((RigidBody*)(simul[o].get())) );
//contact.contactPoint.insert( contact.contactPoint.end(), ((RigidBody*)(simul[o].get()))->getPosition()+(1.0f/2.0f)*midline);
contact.normal.insert( contact.normal.end(), (1.0f/magnitude)*midline);
c.insert(c.end(), contact );
}
}
}
}
}
}
}
}
void process() {
long i, j, k;
line l, l2;
point la, lb, c;
ans = 0;
p[n] = p[0];
for (i = 0; i < n; i++)
for (j = i + 1; j < n; j++) {
l = midline(p[i], p[j]);
for (k = i; k != j; k = (k + 1) % n) {
if (cross(p[k], p[k + 1], l)) {
la = crosspoint(straightline(p[k], p[k + 1]), l);
break;
}
}
if (k == j) printf("ERROR\n");
for (k = j; k != i; k = (k + 1) % n) {
if (cross(p[k], p[k + 1], l)) {
lb = crosspoint(straightline(p[k], p[k + 1]), l);
break;
}
}
if (k == i) printf("ERROR\n");
check(la, 'e');
check(lb, 'e');
for (k = 0; k < n; k++)
if (k != i && k != j) {
l2 = midline(p[i], p[k]);
if (cross(la, lb, l2)) {
c = crosspoint(l2, l);
check(c, 'i');
}
}
}
printf("%.3lf\n", sqrt(ans));
}