void CollisionWorld::addContact(CollisionObject* a, CollisionObject* b, Contact* c)
{
assert(a != b);
ContactManifold* m = getManifold(a, b);
if (m->getA() == b)
{
c->mNormal = -c->mNormal;
Vector3 r1 = c->mRcp1;
c->mRcp1 = c->mRcp2;
c->mRcp2 = r1;
}
m->add(c);
}
int peanoclaw::Area::getAreasOverlappedByRemoteGhostlayers(
const tarch::la::Vector<THREE_POWER_D_MINUS_ONE, int>& adjacentRanks,
tarch::la::Vector<THREE_POWER_D_MINUS_ONE, int> overlapOfRemoteGhostlayers,
const tarch::la::Vector<DIMENSIONS, int>& subdivisionFactor,
int rank,
Area areas[THREE_POWER_D_MINUS_ONE]
) {
logTraceInWith2Arguments("getAreasOverlappedByRemoteGhostlayers(...)", adjacentRanks, overlapOfRemoteGhostlayers);
int numberOfAreas = 0;
bool oneAreaCoversCompleteSubgrid = false;
for(int dimensionality = 0; dimensionality < DIMENSIONS; dimensionality++) {
int numberOfManifolds = getNumberOfManifolds(dimensionality);
for(int manifoldIndex = 0; manifoldIndex < numberOfManifolds; manifoldIndex++) {
tarch::la::Vector<DIMENSIONS, int> manifoldPosition = getManifold(dimensionality, manifoldIndex);
int manifoldEntry = linearizeManifoldPosition(manifoldPosition);
logDebug("getAreasOverlappedByRemoteGhostlayers(...)", "Manifold " << manifoldPosition << " of dimensions " << dimensionality
<< ": entry=" << manifoldEntry << ", rank=" << adjacentRanks[manifoldEntry] << ", overlap=" << overlapOfRemoteGhostlayers[manifoldEntry]);
if(adjacentRanks[manifoldEntry] == rank && overlapOfRemoteGhostlayers[manifoldEntry] > 0) {
//Reduce lower-dimensional manifolds
bool canBeOmitted = checkHigherDimensionalManifoldForOverlap(
adjacentRanks,
overlapOfRemoteGhostlayers,
manifoldPosition,
dimensionality,
manifoldEntry,
rank
);
//Restrict size and offset by higher-dimensional manifolds
logDebug("getAreasOverlappedByRemoteGhostlayers(...)","Testing manifold " << manifoldPosition << " of dimensions " << dimensionality
<< ": " << (canBeOmitted ? "omitting" : "consider"));
if(!canBeOmitted) {
tarch::la::Vector<DIMENSIONS, int> size;
tarch::la::Vector<DIMENSIONS, int> offset;
//Initialise size and offset
for(int d = 0; d < DIMENSIONS; d++) {
size(d) = (manifoldPosition(d) == 0) ? subdivisionFactor(d) : std::min(subdivisionFactor(d), overlapOfRemoteGhostlayers[manifoldEntry]);
offset(d) = (manifoldPosition(d) == 1) ? subdivisionFactor(d) - size(d) : 0;
}
//TODO unterweg debug
// std::cout << "offset: " << offset << ", size: " << size << std::endl;
for(int adjacentDimensionality = dimensionality - 1; adjacentDimensionality >= 0; adjacentDimensionality--) {
int numberOfAdjacentManifolds = getNumberOfAdjacentManifolds(manifoldPosition, dimensionality, adjacentDimensionality);
for(int adjacentManifoldIndex = 0; adjacentManifoldIndex < numberOfAdjacentManifolds; adjacentManifoldIndex++) {
tarch::la::Vector<DIMENSIONS, int> adjacentManifoldPosition = getIndexOfAdjacentManifold(
manifoldPosition,
dimensionality,
adjacentDimensionality,
adjacentManifoldIndex
);
//TODO unterweg debug
// std::cout << "adj. manifold " << adjacentManifoldPosition << std::endl;
int adjacentEntry = linearizeManifoldPosition(adjacentManifoldPosition);
if(adjacentRanks[adjacentEntry] == rank) {
for(int d = 0; d < DIMENSIONS; d++) {
if(manifoldPosition(d) == 0) {
if(adjacentManifoldPosition(d) < 0) {
int overlap = std::max(0, overlapOfRemoteGhostlayers[adjacentEntry] - offset(d));
offset(d) += overlap;
size(d) -= overlap;
//TODO unterweg debug
// std::cout << "Reducing bottom " << overlap << std::endl;
} else if(adjacentManifoldPosition(d) > 0) {
assertion2(adjacentManifoldPosition(d) > 0, adjacentManifoldPosition, d);
int overlap = std::max(0, offset(d) + size(d) - (subdivisionFactor(d) - overlapOfRemoteGhostlayers[adjacentEntry]));
size(d) -= overlap;
//TODO unterweg debug
// std::cout << "Reducing top " << overlap << std::endl;
}
}
}
}
}
}
logDebug("getAreasOverlappedByRemoteGhostlayers(...)", "offset: " << offset << ", size: " << size << ", dimensionality=" << dimensionality << ", manifoldIndex=" << manifoldIndex << ", manifoldEntry=" << manifoldEntry);
if(tarch::la::allGreater(size, 0)) {
areas[numberOfAreas++] = Area(offset, size);
oneAreaCoversCompleteSubgrid |= (tarch::la::volume(size) >= tarch::la::volume(subdivisionFactor));
assertion1(tarch::la::allGreaterEquals(offset, 0), offset);
assertion3(tarch::la::allGreaterEquals(subdivisionFactor, offset + size), offset, size, subdivisionFactor);
}
}
}
}
}
//Optimize areas
if(oneAreaCoversCompleteSubgrid) {
numberOfAreas = 1;
areas[0] = Area(0, subdivisionFactor);
}
logTraceOutWith2Arguments("getAreasOverlappedByRemoteGhostlayers(...)", numberOfAreas, areas);
return numberOfAreas;
}
void CollisionWorld::step(float dt)
{
//check for deleted objects and remove them
m_bodies.erase(std::remove_if(m_bodies.begin(), m_bodies.end(), [](const Body::Ptr& p)
{
return p->deleted();
}), m_bodies.end());
m_constraints.erase(std::remove_if(m_constraints.begin(), m_constraints.end(), [](const Constraint& c)
{
return c.deleted();
}), m_constraints.end());
//check for collision pairs and add to list
//TODO we could narrow this down with space partitioning
//like a quad tree, but probably not necessary in this game
m_collisions.clear();
for (const auto& poA : m_bodies)
{
poA->m_footSenseCount = 0u;
poA->m_footSenseMask = 0u;
for (const auto& poB : m_bodies)
{
if (poA.get() != poB.get())
{
//primary collision between bounding boxes
if (poA->m_aabb.intersects(poB->m_aabb))
{
//minmax assures that as the lowest values is always first in the set
//that each collision pair only gets inserted once
m_collisions.insert(std::minmax(poA.get(), poB.get()));
}
//secondary collisions with sensor boxes
if (poA->m_footSensor.intersects(poB->m_aabb))
{
poA->m_footSenseCount++;
poA->m_footSenseMask |= poB->m_type;
}
}
}
}
//resolve collision for each pair
for (const auto& pair : m_collisions)
{
//call state resolve
auto man = getManifold(pair);
pair.second->m_behaviour->resolve(man, pair.first);
man.z = -man.z;
pair.first->m_behaviour->resolve(man, pair.second);
}
//apply any constraints to their respective bodies
for (auto& c : m_constraints)
{
c.update(dt);
}
//update any parent node positions
for (auto& b : m_bodies)
{
b->applyGravity(m_gravity);
b->step(dt);
}
}
