void add_simplices(SimplexVector& sv, int d, const PointContainer& points) { PointIndex indices[d+1]; for (int i = 0; i < d+1; ++i) indices[i] = d - i; while(indices[d] < points.size() - d) { // Add simplex Miniball mb(points[indices[0]].dim()); Simplex s; for (int i = 0; i < d+1; ++i) { s.add(indices[i]); mb.check_in(points[indices[i]]); } mb.build(); s.set_value(mb.squared_radius()); sv.push_back(s); // Advance indices for (int i = 0; i < d+1; ++i) { ++indices[i]; if (indices[i] < points.size() - i) { for (int j = i-1; j >= 0; --j) indices[j] = indices[j+1] + 1; break; } } } }
PhysicsCollisionData PhysicsColliderSphere::collideWith(PhysicsCollider *other) { if (other->getType() == PhysicsColliderTypeSphere) { PhysicsColliderSphere *_other = static_cast<PhysicsColliderSphere *>(other); float centerDistance = glm::length(this->getPosition() - _other->getPosition()); float radiusSum = this->getRadius() + _other->getRadius(); glm::vec3 normal = glm::normalize(this->getPosition() - _other->getPosition()); if (radiusSum < centerDistance) return PhysicsCollisionData(false, 0); else { return PhysicsCollisionData(normal, true, centerDistance - radiusSum); } } else if (other->getType() == PhysicsColliderTypeAABB) { PhysicsColliderAABB *_other = static_cast<PhysicsColliderAABB *>(other); glm::vec3 separatingAxis = this->getPosition() - _other->getPosition(); float distance = glm::length(separatingAxis); separatingAxis = glm::normalize(separatingAxis); if (separatingAxis.x >= separatingAxis.y && separatingAxis.x >= separatingAxis.z) separatingAxis /= separatingAxis.x; else if (separatingAxis.y >= separatingAxis.x && separatingAxis.y >= separatingAxis.z) separatingAxis /= separatingAxis.y; else separatingAxis /= separatingAxis.z; separatingAxis.x *= _other->getWidth() / 2.0f; separatingAxis.y *= _other->getHeight() / 2.0f; separatingAxis.z *= _other->getDepth() / 2.0f; if (distance <= (this->getRadius() + glm::length(separatingAxis))) return PhysicsCollisionData(true, distance); else return PhysicsCollisionData(false, distance); } else if (other->getType() == PhysicsColliderTypePlane) { PhysicsColliderPlane *_other = static_cast<PhysicsColliderPlane *>(other); float distanceFromCenter = glm::dot(_other->getNormal(), this->getPosition()) - _other->getDistance(); float distanceFromSphere = distanceFromCenter - this->getRadius(); if (distanceFromSphere < 0) return PhysicsCollisionData(true, distanceFromSphere); else return PhysicsCollisionData(false, distanceFromSphere); } else if (other->getType() == PhysicsColliderTypeMesh) { PhysicsColliderMesh *_other = static_cast<PhysicsColliderMesh *>(other); Simplex simplex; glm::vec3 direction = glm::vec3(1, 1, 1); glm::vec3 a = support(_other, this, direction, simplex); simplex.add(a); direction = -a; int max = 10; for (int i = 0; i < max; i++) { glm::vec3 point = support(_other, this, direction, simplex); if (glm::dot(point, direction) < 0) { return PhysicsCollisionData(false, 0); } else { simplex.add(point); if (processSimplex(simplex, direction) == true) { return PhysicsCollisionData(true, 0); } } } } return PhysicsCollisionData(false, 0); }