ecker/engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

more refinements and optimizations for the physics

Browse Source
This commit is contained in:
ecker 2025-09-05 19:08:07 -05:00
parent fff258917e
commit cb36936411
15 changed files with 286 additions and 257 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
engine/inc/uf/utils/math/physics/impl.h
View File

@ -107,27 +107,27 @@ namespace pod {
struct Collider { struct Collider {
// what it is // what it is
enum CategoryMask : uint32_t { enum CategoryMask : uint32_t {
CATEGORY_NONE = 0, CATEGORY_NONE = 0,
CATEGORY_STATIC = 1 << 0, CATEGORY_STATIC = 1 << 0,
CATEGORY_DYNAMIC = 1 << 1, CATEGORY_DYNAMIC = 1 << 1,
CATEGORY_PLAYER = 1 << 2, CATEGORY_PLAYER = 1 << 2,
CATEGORY_NPC = 1 << 3, CATEGORY_NPC = 1 << 3,
CATEGORY_TRIGGER = 1 << 4, CATEGORY_TRIGGER = 1 << 4,
CATEGORY_PROJECTILE = 1 << 5, CATEGORY_PROJECTILE = 1 << 5,
CATEGORY_CHARACTER = CATEGORY_PLAYER | CATEGORY_NPC, CATEGORY_CHARACTER = CATEGORY_PLAYER | CATEGORY_NPC,
CATEGORY_ALL = 0xFFFFFFFF CATEGORY_ALL = 0xFFFFFFFF
}; };
// what it collides with // what it collides with
enum CollisionMask : uint32_t { enum CollisionMask : uint32_t {
MASK_NONE = 0, MASK_NONE = 0,
MASK_STATIC = CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC | CATEGORY_PROJECTILE, MASK_STATIC = CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC | CATEGORY_PROJECTILE,
MASK_DYNAMIC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC, MASK_DYNAMIC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
MASK_PLAYER = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_NPC | CATEGORY_PROJECTILE, MASK_PLAYER = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_NPC | CATEGORY_PROJECTILE,
MASK_NPC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_PROJECTILE, MASK_NPC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_PROJECTILE,
MASK_TRIGGER = CATEGORY_PLAYER | CATEGORY_NPC, MASK_TRIGGER = CATEGORY_PLAYER | CATEGORY_NPC,
MASK_PROJECTILE = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC, MASK_PROJECTILE = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
MASK_CHARACTER = MASK_PLAYER | MASK_NPC, MASK_CHARACTER = MASK_PLAYER | MASK_NPC,
MASK_ALL = 0xFFFFFFFF MASK_ALL = 0xFFFFFFFF
}; };
pod::ShapeType type; pod::ShapeType type;
@ -141,7 +141,7 @@ namespace pod {
pod::Capsule capsule; pod::Capsule capsule;
pod::TriangleWithNormal triangle; pod::TriangleWithNormal triangle;
pod::MeshBVH mesh; pod::MeshBVH mesh;
} u; };
}; };
struct PhysicsMaterial { struct PhysicsMaterial {
@ -217,7 +217,8 @@ namespace pod {
struct Island { struct Island {
bool awake = true; bool awake = true;
uf::stl::vector<pod::PhysicsBody*> bodies; uf::stl::vector<uint32_t> indices;
pod::BVH::pairs_t pairs;
}; };
struct World { struct World {

20
engine/src/utils/math/physics/aabb.inl
View File

@ -21,7 +21,7 @@ namespace {
std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body ) { std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body ) {
const auto transform = ::getTransform( body ); const auto transform = ::getTransform( body );
const auto& capsule = body.collider.u.capsule; const auto& capsule = body.collider.capsule;
const pod::Vector3f up = uf::quaternion::rotate( transform.orientation, pod::Vector3f{0,1,0} ); const pod::Vector3f up = uf::quaternion::rotate( transform.orientation, pod::Vector3f{0,1,0} );
// segment defines the cylinder axis only (ignore spherical ends) // segment defines the cylinder axis only (ignore spherical ends)
@ -35,25 +35,25 @@ namespace {
switch ( body.collider.type ) { switch ( body.collider.type ) {
case pod::ShapeType::AABB: { case pod::ShapeType::AABB: {
return { return {
transform.position + body.collider.u.aabb.min, transform.position + body.collider.aabb.min,
transform.position + body.collider.u.aabb.max, transform.position + body.collider.aabb.max,
}; };
} break; } break;
case pod::ShapeType::SPHERE: { case pod::ShapeType::SPHERE: {
return { return {
transform.position - body.collider.u.sphere.radius, transform.position - body.collider.sphere.radius,
transform.position + body.collider.u.sphere.radius, transform.position + body.collider.sphere.radius,
}; };
} break; } break;
case pod::ShapeType::CAPSULE: { case pod::ShapeType::CAPSULE: {
auto [ p1, p2 ] = ::getCapsuleSegment( body ); auto [ p1, p2 ] = ::getCapsuleSegment( body );
return ::computeSegmentAABB( p1, p2, body.collider.u.capsule.radius ); return ::computeSegmentAABB( p1, p2, body.collider.capsule.radius );
} break; } break;
case pod::ShapeType::MESH: { case pod::ShapeType::MESH: {
if ( body.collider.u.mesh.bvh && !body.collider.u.mesh.bvh->nodes.empty() ) if ( body.collider.mesh.bvh && !body.collider.mesh.bvh->nodes.empty() )
return { return {
transform.position + body.collider.u.mesh.bvh->nodes[0].bounds.min, transform.position + body.collider.mesh.bvh->nodes[0].bounds.min,
transform.position + body.collider.u.mesh.bvh->nodes[0].bounds.max, transform.position + body.collider.mesh.bvh->nodes[0].bounds.max,
}; };
} break; } break;
default: { default: {
@ -193,7 +193,7 @@ namespace {
auto overlaps = uf::vector::min( A.max, B.max ) - uf::vector::max( A.min, B.min ); auto overlaps = uf::vector::min( A.max, B.max ) - uf::vector::max( A.min, B.min );
// determine collision axis = smallest overlap // determine collision axis = smallest overlap
int axis = -1; auto axis = -1;
float minOverlap = FLT_MAX; float minOverlap = FLT_MAX;
for ( auto i = 0; i < 3; ++i ) { for ( auto i = 0; i < 3; ++i ) {
if ( overlaps[i] < minOverlap ) { if ( overlaps[i] < minOverlap ) {

252
engine/src/utils/math/physics/bvh.inl
View File

@ -1,6 +1,6 @@
// BVH // BVH
namespace { namespace {
int buildBVHNode( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, int start, int end, int capacity = 2 ) { int32_t buildBVHNode( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, int32_t start, int32_t end, int32_t capacity = 2 ) {
pod::BVH::Node node{}; pod::BVH::Node node{};
node.left = -1; node.left = -1;
node.right = -1; node.right = -1;
@ -11,19 +11,19 @@ namespace {
// compute bounds of this node // compute bounds of this node
for ( auto i = start + 1; i < end; ++i) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] ); for ( auto i = start + 1; i < end; ++i) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] );
int count = end - start; int32_t count = end - start;
if ( count <= capacity ) { if ( count <= capacity ) {
// leaf // leaf
node.start = start; node.start = start;
node.count = count; node.count = count;
int index = (int) bvh.nodes.size(); int32_t index = (int32_t) bvh.nodes.size();
bvh.nodes.emplace_back(node); bvh.nodes.emplace_back(node);
return index; return index;
} }
// choose split axis by largest extent // choose split axis by largest extent
auto extent = node.bounds.max - node.bounds.min; auto extent = node.bounds.max - node.bounds.min;
int axis = (extent.x > extent.y && extent.x > extent.z) ? 0 : (extent.y > extent.z ? 1 : 2); auto axis = (extent.x > extent.y && extent.x > extent.z) ? 0 : (extent.y > extent.z ? 1 : 2);
// sort indices by centroid along axis // sort indices by centroid along axis
std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](uint32_t a, uint32_t b) { std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](uint32_t a, uint32_t b) {
@ -32,8 +32,8 @@ namespace {
return ca < cb; return ca < cb;
}); });
int mid = ( start + end ) / 2; int32_t mid = ( start + end ) / 2;
int index = (int) bvh.nodes.size(); int32_t index = (int32_t) bvh.nodes.size();
bvh.nodes.emplace_back( node ); // insert now, gets filled later bvh.nodes.emplace_back( node ); // insert now, gets filled later
node.left = ::buildBVHNode( bvh, bounds, start, mid, capacity ); node.left = ::buildBVHNode( bvh, bounds, start, mid, capacity );
@ -42,10 +42,10 @@ namespace {
return index; return index;
} }
int buildBVHNode_SAH( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, int start, int end, int capacity = 4 ) { int32_t buildBVHNode_SAH( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, int32_t start, int32_t end, int32_t capacity = 4 ) {
struct Bin { struct Bin {
pod::AABB bounds; pod::AABB bounds;
int count = 0; int32_t count = 0;
}; };
pod::BVH::Node node{}; pod::BVH::Node node{};
@ -57,20 +57,20 @@ namespace {
for ( auto i = start + 1; i < end; ++i ) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] ); for ( auto i = start + 1; i < end; ++i ) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] );
int count = end - start; int32_t count = end - start;
if ( count <= capacity ) { if ( count <= capacity ) {
node.count = count; node.count = count;
int index = (int) bvh.nodes.size(); int32_t index = (int32_t) bvh.nodes.size();
bvh.nodes.emplace_back(node); bvh.nodes.emplace_back(node);
return index; return index;
} }
constexpr int numBins = 16; constexpr auto numBins = 16;
static thread_local Bin bins[numBins]; static thread_local Bin bins[numBins];
for ( auto i = 0; i < numBins; i++ ) bins[i] = {}; for ( auto i = 0; i < numBins; i++ ) bins[i] = {};
auto extent = node.bounds.max - node.bounds.min; auto extent = node.bounds.max - node.bounds.min;
int bestAxis = -1, bestSplit = -1; auto bestAxis = -1, bestSplit = -1;
float bestCost = std::numeric_limits<float>::infinity(); float bestCost = std::numeric_limits<float>::infinity();
for ( auto axis = 0; axis < 3; ++axis ) { for ( auto axis = 0; axis < 3; ++axis ) {
@ -81,18 +81,18 @@ namespace {
float scale = (float) numBins / (maxC - minC); float scale = (float) numBins / (maxC - minC);
for ( auto i = start; i < end; ++i ) { for ( auto i = start; i < end; ++i ) {
int idx = bvh.indices[i]; int32_t idx = bvh.indices[i];
float c = ::aabbCenter( bounds[idx] )[axis]; float c = ::aabbCenter( bounds[idx] )[axis];
int binID = std::min(numBins - 1, (int)((c - minC) * scale)); int32_t binID = std::min(numBins - 1, (int32_t)((c - minC) * scale));
bins[binID].count++; bins[binID].count++;
bins[binID].bounds = ::mergeAabb( bins[binID].bounds, bounds[idx] ); bins[binID].bounds = ::mergeAabb( bins[binID].bounds, bounds[idx] );
} }
pod::AABB leftBounds[numBins], rightBounds[numBins]; pod::AABB leftBounds[numBins], rightBounds[numBins];
int leftCount[numBins] = {}, rightCount[numBins] = {}; int32_t leftCount[numBins] = {}, rightCount[numBins] = {};
pod::AABB acc; pod::AABB acc;
int cnt = 0; int32_t cnt = 0;
for ( auto i = 0; i < numBins; i++ ) { for ( auto i = 0; i < numBins; i++ ) {
if ( bins[i].count > 0 ) acc = (cnt == 0) ? bins[i].bounds : ::mergeAabb( acc, bins[i].bounds ); if ( bins[i].count > 0 ) acc = (cnt == 0) ? bins[i].bounds : ::mergeAabb( acc, bins[i].bounds );
cnt += bins[i].count; cnt += bins[i].count;
@ -127,7 +127,7 @@ namespace {
// fallback: no valid split → make leaf // fallback: no valid split → make leaf
if ( bestAxis == -1 ) { if ( bestAxis == -1 ) {
node.count = count; node.count = count;
int index = (int) bvh.nodes.size(); int32_t index = (int32_t) bvh.nodes.size();
bvh.nodes.emplace_back(node); bvh.nodes.emplace_back(node);
return index; return index;
} }
@ -136,23 +136,23 @@ namespace {
float maxC = node.bounds.max[bestAxis]; float maxC = node.bounds.max[bestAxis];
float scale = (float) numBins / (maxC - minC); float scale = (float) numBins / (maxC - minC);
auto midIt = std::partition( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](int idx) { auto midIt = std::partition( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](int32_t idx) {
float c = ::aabbCenter( bounds[idx])[bestAxis ]; float c = ::aabbCenter( bounds[idx])[bestAxis ];
int binID = std::min(numBins - 1, (int)((c - minC) * scale)); int32_t binID = std::min(numBins - 1, (int32_t)((c - minC) * scale));
return binID <= bestSplit; return binID <= bestSplit;
}); });
int mid = (int) ( midIt - bvh.indices.begin() ); int32_t mid = (int32_t) ( midIt - bvh.indices.begin() );
// if partition failed (all left or all right), force leaf // if partition failed (all left or all right), force leaf
if ( mid == start || mid == end ) { if ( mid == start || mid == end ) {
node.count = count; node.count = count;
int index = (int) bvh.nodes.size(); int32_t index = (int32_t) bvh.nodes.size();
bvh.nodes.emplace_back(node); bvh.nodes.emplace_back(node);
return index; return index;
} }
int index = (int) bvh.nodes.size(); int32_t index = (int32_t) bvh.nodes.size();
bvh.nodes.emplace_back(node); bvh.nodes.emplace_back(node);
node.left = ::buildBVHNode_SAH( bvh, bounds, start, mid, capacity ); node.left = ::buildBVHNode_SAH( bvh, bounds, start, mid, capacity );
@ -161,7 +161,7 @@ namespace {
return index; return index;
} }
void buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies, int capacity = 2, bool filters = false, bool filterType = false ) { void buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies, int32_t capacity = 2, bool filters = false, bool filterType = false ) {
if ( bodies.empty() ) return; if ( bodies.empty() ) return;
bvh.indices.clear(); bvh.indices.clear();
@ -175,7 +175,7 @@ namespace {
for ( auto i = 0; i < bodies.size(); ++i ) { for ( auto i = 0; i < bodies.size(); ++i ) {
if ( filters && bodies[i]->isStatic != filterType ) continue; if ( filters && bodies[i]->isStatic != filterType ) continue;
bounds[i] = ::computeAABB(*bodies[i]); bounds[i] = bodies[i]->bounds;
bvh.indices.emplace_back(i); bvh.indices.emplace_back(i);
} }
@ -191,8 +191,8 @@ namespace {
bvh.dirty = false; bvh.dirty = false;
} }
void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int capacity = 4 ) { void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int32_t capacity = 4 ) {
int triangles = mesh.index.count / 3; uint32_t triangles = mesh.index.count / 3;
bvh.indices.clear(); bvh.indices.clear();
bvh.nodes.clear(); bvh.nodes.clear();
@ -235,22 +235,23 @@ namespace {
} }
namespace { namespace {
pod::BVH::UpdatePolicy::Decision decideBVHUpdate( const pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies, const pod::BVH::UpdatePolicy& policy, int frameCounter ) { pod::BVH::UpdatePolicy::Decision decideBVHUpdate( const pod::BVH& bvh, uf::stl::vector<pod::PhysicsBody*>& bodies, const pod::BVH::UpdatePolicy& policy, size_t frameCounter ) {
if ( bvh.indices.empty() || bvh.nodes.empty() || bvh.dirty ) { // BVH is not built
UF_MSG_DEBUG("Force rebuild, bvh.indices.empty={}, bvh.nodes.empty={}, bvh.dirty={}", bvh.indices.empty(), bvh.nodes.empty(), bvh.dirty ); if ( bvh.indices.empty() || bvh.nodes.empty() ) {
return pod::BVH::UpdatePolicy::Decision::REBUILD; return pod::BVH::UpdatePolicy::Decision::REBUILD;
} }
if ( bodies.empty() ) return pod::BVH::UpdatePolicy::Decision::NONE; if ( bodies.empty() ) return pod::BVH::UpdatePolicy::Decision::NONE;
int dirtyCount = 0; uint32_t dirtyCount = 0;
float oldRootArea = ::aabbSurfaceArea( bvh.nodes[0].bounds ); float oldRootArea = ::aabbSurfaceArea( bvh.nodes[0].bounds );
// check each body // update/check each body
for ( auto idx : bvh.indices ) { for ( auto idx : bvh.indices ) {
const auto* body = bodies[idx]; auto& body = *bodies[idx];
pod::AABB newBounds = ::computeAABB(*body); pod::AABB oldBounds = body.bounds;
pod::AABB oldBounds = body->bounds; body.bounds = ::computeAABB( body );
pod::AABB newBounds = body.bounds;
// compute displacement relative to size // compute displacement relative to size
pod::Vector3f oldCenter = ( oldBounds.min + oldBounds.max ) * 0.5f; pod::Vector3f oldCenter = ( oldBounds.min + oldBounds.max ) * 0.5f;
@ -272,10 +273,11 @@ namespace {
} }
float newRootArea = ::aabbSurfaceArea( newRoot ); float newRootArea = ::aabbSurfaceArea( newRoot );
if ( dirtyRatio > policy.dirtyRatioThreshold || newRootArea > oldRootArea * policy.overlapThreshold || frameCounter % policy.maxFramesBeforeRebuild == 0 ) { // BVH is too out of date, rebuild it
UF_MSG_DEBUG( "Rebuild, dirtyRatio={}, oldRootArea={}, newRootArea={}, frameCounter={}", dirtyRatio, oldRootArea, newRootArea, frameCounter ); if ( bvh.dirty || dirtyRatio > policy.dirtyRatioThreshold || newRootArea > oldRootArea * policy.overlapThreshold || frameCounter % policy.maxFramesBeforeRebuild == 0 ) {
return pod::BVH::UpdatePolicy::Decision::REBUILD; return pod::BVH::UpdatePolicy::Decision::REBUILD;
} }
// bodies moved, refit the BVH instead
if ( dirtyCount > 0 ) return pod::BVH::UpdatePolicy::Decision::REFIT; if ( dirtyCount > 0 ) return pod::BVH::UpdatePolicy::Decision::REFIT;
return pod::BVH::UpdatePolicy::Decision::NONE; return pod::BVH::UpdatePolicy::Decision::NONE;
} }
@ -293,14 +295,14 @@ namespace {
// leaf node: recompute bounds from bodies // leaf node: recompute bounds from bodies
node.bounds = bounds[bvh.indices[node.start]]; node.bounds = bounds[bvh.indices[node.start]];
for ( int j = 1; j < node.count; j++ ) { for ( auto j = 1; j < node.count; j++ ) {
node.bounds = ::mergeAabb(node.bounds, bounds[bvh.indices[node.start + j]] ); node.bounds = ::mergeAabb(node.bounds, bounds[bvh.indices[node.start + j]] );
} }
} }
} }
// update internal nodes bottom-up // update internal nodes bottom-up
for ( int i = (int) bvh.nodes.size() - 1; i >= 0; i-- ) { for ( int32_t i = (int32_t) bvh.nodes.size() - 1; i >= 0; i-- ) {
auto& node = bvh.nodes[i]; auto& node = bvh.nodes[i];
// internal node // internal node
if ( node.count == 0 ) { if ( node.count == 0 ) {
@ -321,19 +323,19 @@ namespace {
// leaf node: recompute bounds from bodies // leaf node: recompute bounds from bodies
auto nodeID = bvh.indices[node.start]; auto nodeID = bvh.indices[node.start];
node.bounds = ::computeAABB( *bodies[nodeID] ); node.bounds = bodies[nodeID]->bounds;
node.asleep = !bodies[nodeID]->activity.awake; node.asleep = !bodies[nodeID]->activity.awake;
for ( int j = 1; j < node.count; j++ ) { for ( auto j = 1; j < node.count; j++ ) {
auto bodyID = bvh.indices[node.start + j]; auto bodyID = bvh.indices[node.start + j];
node.bounds = ::mergeAabb(node.bounds, ::computeAABB( *bodies[bodyID] ) ); node.bounds = ::mergeAabb(node.bounds, bodies[bodyID]->bounds );
node.asleep = node.asleep && !bodies[bodyID]->activity.awake; node.asleep = node.asleep && !bodies[bodyID]->activity.awake;
} }
} }
} }
// update internal nodes bottom-up // update internal nodes bottom-up
for ( int i = (int) bvh.nodes.size() - 1; i >= 0; i-- ) { for ( int32_t i = (int32_t) bvh.nodes.size() - 1; i >= 0; i-- ) {
auto& node = bvh.nodes[i]; auto& node = bvh.nodes[i];
// internal node // internal node
if ( node.count == 0 ) { if ( node.count == 0 ) {
@ -346,7 +348,7 @@ namespace {
} }
void refitBVH( pod::BVH& bvh, const uf::Mesh& mesh ) { void refitBVH( pod::BVH& bvh, const uf::Mesh& mesh ) {
int triangles = mesh.index.count / 3; uint32_t triangles = mesh.index.count / 3;
uf::stl::vector<pod::AABB> bounds; uf::stl::vector<pod::AABB> bounds;
bounds.reserve( triangles ); bounds.reserve( triangles );
@ -373,12 +375,12 @@ namespace {
} }
namespace { namespace {
int flattenBVH( pod::BVH& bvh, int nodeID ) { int32_t flattenBVH( pod::BVH& bvh, int32_t nodeID ) {
if ( nodeID == 0 ) bvh.flattened.reserve(bvh.nodes.size()); if ( nodeID == 0 ) bvh.flattened.reserve(bvh.nodes.size());
const auto& node = bvh.nodes[nodeID]; const auto& node = bvh.nodes[nodeID];
int flatID = (int) bvh.flattened.size(); int32_t flatID = (int32_t) bvh.flattened.size();
bvh.flattened.emplace_back(); // placeholder bvh.flattened.emplace_back(); // placeholder
pod::BVH::FlatNode flat{}; pod::BVH::FlatNode flat{};
@ -401,8 +403,8 @@ namespace {
flat.start = -1; flat.start = -1;
flat.count = 0; flat.count = 0;
int leftID = ::flattenBVH( bvh, node.left ); int32_t leftID = ::flattenBVH( bvh, node.left );
int rightID = ::flattenBVH( bvh, node.right ); int32_t rightID = ::flattenBVH( bvh, node.right );
flat.skipIndex = rightID; // skip entire subtree flat.skipIndex = rightID; // skip entire subtree
bvh.flattened[flatID] = flat; bvh.flattened[flatID] = flat;
@ -413,7 +415,7 @@ namespace {
namespace { namespace {
// collects a list of nodes that are overlapping with each other // collects a list of nodes that are overlapping with each other
void traverseNodePair(const pod::BVH& bvh, int nodeAID, int nodeBID, pod::BVH::pairs_t& pairs) { void traverseNodePair(const pod::BVH& bvh, int32_t nodeAID, int32_t nodeBID, pod::BVH::pairs_t& pairs) {
const auto& nodeA = bvh.nodes[nodeAID]; const auto& nodeA = bvh.nodes[nodeAID];
const auto& nodeB = bvh.nodes[nodeBID]; const auto& nodeB = bvh.nodes[nodeBID];
@ -422,8 +424,8 @@ namespace {
if ( nodeA.count > 0 && nodeB.count > 0 ) { if ( nodeA.count > 0 && nodeB.count > 0 ) {
for ( auto i = 0; i < nodeA.count; ++i ) { for ( auto i = 0; i < nodeA.count; ++i ) {
for ( auto j = 0; j < nodeB.count; ++j ) { for ( auto j = 0; j < nodeB.count; ++j ) {
int bodyA = bvh.indices[nodeA.start + i]; int32_t bodyA = bvh.indices[nodeA.start + i];
int bodyB = bvh.indices[nodeB.start + j]; int32_t bodyB = bvh.indices[nodeB.start + j];
if ( bodyA == bodyB ) continue; if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
@ -443,7 +445,7 @@ namespace {
} }
} }
// collects a list of nodes from each BVH that are overlapping with each other (for mesh v mesh) // collects a list of nodes from each BVH that are overlapping with each other (for mesh v mesh)
void traverseNodePair( const pod::BVH& bvhA, int nodeAID, const pod::BVH& bvhB, int nodeBID, pod::BVH::pairs_t& pairs ) { void traverseNodePair( const pod::BVH& bvhA, int32_t nodeAID, const pod::BVH& bvhB, int32_t nodeBID, pod::BVH::pairs_t& pairs ) {
const auto& nodeA = bvhA.nodes[nodeAID]; const auto& nodeA = bvhA.nodes[nodeAID];
const auto& nodeB = bvhB.nodes[nodeBID]; const auto& nodeB = bvhB.nodes[nodeBID];
@ -452,8 +454,8 @@ namespace {
if ( nodeA.count > 0 && nodeB.count > 0 ) { if ( nodeA.count > 0 && nodeB.count > 0 ) {
for ( auto i = 0; i < nodeA.count; ++i ) { for ( auto i = 0; i < nodeA.count; ++i ) {
for ( auto j = 0; j < nodeB.count; ++j ) { for ( auto j = 0; j < nodeB.count; ++j ) {
int bodyA = bvhA.indices[nodeA.start + i]; int32_t bodyA = bvhA.indices[nodeA.start + i];
int bodyB = bvhB.indices[nodeB.start + j]; int32_t bodyB = bvhB.indices[nodeB.start + j];
if ( bodyA == bodyB ) continue; if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
@ -473,14 +475,14 @@ namespace {
} }
} }
void traverseBVH( const pod::BVH& bvh, int nodeID, pod::BVH::pairs_t& pairs ) { void traverseBVH( const pod::BVH& bvh, int32_t nodeID, pod::BVH::pairs_t& pairs ) {
const auto& node = bvh.nodes[nodeID]; const auto& node = bvh.nodes[nodeID];
if ( node.count > 0 ) { if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i ) { for ( auto i = 0; i < node.count; ++i ) {
for ( auto j = i + 1; j < node.count; ++j ) { for ( auto j = i + 1; j < node.count; ++j ) {
int bodyA = bvh.indices[node.start + i]; int32_t bodyA = bvh.indices[node.start + i];
int bodyB = bvh.indices[node.start + j]; int32_t bodyB = bvh.indices[node.start + j];
if ( bodyA == bodyB ) continue; if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
@ -524,7 +526,7 @@ namespace {
stack.push(0); stack.push(0);
while ( !stack.empty() ) { while ( !stack.empty() ) {
int idx = stack.top(); stack.pop(); int32_t idx = stack.top(); stack.pop();
auto& node = bvh.nodes[idx]; auto& node = bvh.nodes[idx];
if ( node.asleep || !::aabbOverlap( bounds, node.bounds ) ) continue; if ( node.asleep || !::aabbOverlap( bounds, node.bounds ) ) continue;
@ -541,7 +543,7 @@ namespace {
} }
// query a BVH with an AABB via recursion // query a BVH with an AABB via recursion
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& outIndices, int nodeID ) { void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& outIndices, int32_t nodeID ) {
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices ); if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices );
if ( nodeID == 0 ) outIndices.reserve(::reserveCount); if ( nodeID == 0 ) outIndices.reserve(::reserveCount);
@ -570,7 +572,7 @@ namespace {
stack.push(0); stack.push(0);
while ( !stack.empty() ) { while ( !stack.empty() ) {
int idx = stack.top(); stack.pop(); int32_t idx = stack.top(); stack.pop();
const auto& node = bvh.nodes[idx]; const auto& node = bvh.nodes[idx];
float tMin, tMax; float tMin, tMax;
@ -586,7 +588,7 @@ namespace {
} }
} }
// query a BVH with a ray via recursion // query a BVH with a ray via recursion
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& outIndices, int nodeID, float maxDist ) { void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& outIndices, int32_t nodeID, float maxDist ) {
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, ray, outIndices, maxDist ); if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, ray, outIndices, maxDist );
if ( nodeID == 0 ) outIndices.reserve(::reserveCount); if ( nodeID == 0 ) outIndices.reserve(::reserveCount);
@ -615,11 +617,11 @@ namespace {
outPairs.reserve(::reserveCount); outPairs.reserve(::reserveCount);
for ( auto i = 0; i < (int) nodes.size(); ++i ) { for ( auto i = 0; i < nodes.size(); ++i ) {
const auto& nodeA = nodes[i]; const auto& nodeA = nodes[i];
if ( nodeA.count <= 0 || nodeA.asleep ) continue; if ( nodeA.count <= 0 || nodeA.asleep ) continue;
for ( auto j = i + 1; j < (int) nodes.size(); ++j ) { for ( auto j = i + 1; j < nodes.size(); ++j ) {
const auto& nodeB = nodes[j]; const auto& nodeB = nodes[j];
if ( nodeB.count <= 0 || nodeB.asleep ) continue; if ( nodeB.count <= 0 || nodeB.asleep ) continue;
@ -650,11 +652,11 @@ namespace {
outPairs.reserve(::reserveCount); outPairs.reserve(::reserveCount);
for ( auto i = 0; i < (int) nodesA.size(); ++i ) { for ( auto i = 0; i < nodesA.size(); ++i ) {
const auto& nodeA = nodesA[i]; const auto& nodeA = nodesA[i];
if ( nodeA.count <= 0 || nodeA.asleep ) continue; if ( nodeA.count <= 0 || nodeA.asleep ) continue;
for ( auto j = 0; j < (int) nodesB.size(); ++j ) { for ( auto j = 0; j < nodesB.size(); ++j ) {
const auto& nodeB = nodesB[j]; const auto& nodeB = nodesB[j];
if ( nodeB.count <= 0 || nodeB.asleep ) continue; if ( nodeB.count <= 0 || nodeB.asleep ) continue;
@ -678,14 +680,14 @@ namespace {
outIndices.reserve(::reserveCount); outIndices.reserve(::reserveCount);
int idx = 0; int32_t idx = 0;
while ( idx < (int) nodes.size() ) { while ( idx < nodes.size() ) {
const auto& node = nodes[idx]; const auto& node = nodes[idx];
if ( !node.asleep && ::aabbOverlap( bounds, node.bounds ) ) { if ( !node.asleep && ::aabbOverlap( bounds, node.bounds ) ) {
// leaf // leaf
if ( node.count > 0 ) { if ( node.count > 0 ) {
for ( int i = 0; i < node.count; ++i ) { for ( auto i = 0; i < node.count; ++i ) {
outIndices.emplace_back( indices[node.start + i] ); outIndices.emplace_back( indices[node.start + i] );
} }
} }
@ -702,14 +704,14 @@ namespace {
outIndices.reserve(::reserveCount); outIndices.reserve(::reserveCount);
int idx = 0; int32_t idx = 0;
while ( idx < (int) nodes.size() ) { while ( idx < nodes.size() ) {
const auto& node = nodes[idx]; const auto& node = nodes[idx];
float tMin, tMax; float tMin, tMax;
if ( !node.asleep && ::rayAabbIntersect( ray, node.bounds, tMin, tMax ) && tMin <= maxDist ) { if ( !node.asleep && ::rayAabbIntersect( ray, node.bounds, tMin, tMax ) && tMin <= maxDist ) {
// leaf // leaf
if ( node.count > 0 ) { if ( node.count > 0 ) {
for ( int i = 0; i < node.count; ++i ) { for ( auto i = 0; i < node.count; ++i ) {
outIndices.emplace_back( indices[node.start + i] ); outIndices.emplace_back( indices[node.start + i] );
} }
} }
@ -723,47 +725,80 @@ namespace {
} }
namespace { namespace {
struct UnionFind {
uf::stl::vector<int32_t> parent;
uf::stl::vector<int32_t> rank;
UnionFind( int32_t n ) {
parent.resize(n);
rank.resize(n, 0);
for ( auto i = 0; i < n; i++ )
parent[i] = i;
}
int32_t find( int32_t x ) {
if ( parent[x] != x ) parent[x] = find(parent[x]);
return parent[x];
}
void unite( int32_t a, int32_t b ) {
int32_t rootA = find(a);
int32_t rootB = find(b);
if ( rootA == rootB ) return;
// union by rank
if ( rank[rootA] < rank[rootB] ) parent[rootA] = rootB;
else if ( rank[rootA] > rank[rootB] ) parent[rootB] = rootA;
else {
parent[rootB] = rootA;
rank[rootA]++;
}
}
};
void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands ) { void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands ) {
islands.reserve(::reserveCount); UnionFind unionizer(bodies.size());
int n = (int) bodies.size(); // union all pairs
uf::stl::vector<int32_t> visited(n, -1); for ( auto& [a, b] : pairs ) {
unionizer.unite(a, b);
}
for ( auto i = 0; i < n; i++ ) { // map root to island index
if ( visited[i] != -1 ) continue; uf::stl::unordered_map<int32_t, int32_t> rootToIsland;
// new island islands.clear();
pod::Island island = {}; islands.reserve(bodies.size());
uf::stl::stack<int32_t> stack;
stack.push(i);
while ( !stack.empty() ) { for ( auto i = 0; i < bodies.size(); i++ ) {
int idx = stack.top(); stack.pop(); int32_t root = unionizer.find(i);
if ( visited[idx] != -1 ) continue;
visited[idx] = (int) islands.size();
island.bodies.emplace_back( bodies[idx] ); if (rootToIsland.find(root) == rootToIsland.end()) {
rootToIsland[root] = (int32_t) islands.size();
// traverse neighbors islands.emplace_back();
for ( auto& [a, b] : pairs ) {
int neighbor = -1;
if ( a == idx ) neighbor = b;
else if ( b == idx ) neighbor = a;
if ( neighbor != -1 && visited[neighbor] == -1 ) {
stack.push(neighbor);
}
}
} }
islands.emplace_back( std::move( island ) ); int32_t islandID = rootToIsland[root];
islands[islandID].indices.emplace_back( i );
}
// collect pairs per island
for ( auto& [a, b] : pairs ) {
// do not insert these pairs if they're non-colliding
if ( !::shouldCollide( *bodies[a], *bodies[b] ) ) continue;
int32_t root = unionizer.find(a);
int32_t islandID = rootToIsland[root];
islands[islandID].pairs.emplace(a, b);
} }
} }
void updateIsland( pod::Island& island, float dt ) { bool updateIsland( pod::Island& island, uf::stl::vector<pod::PhysicsBody*>& bodies, float dt ) {
bool allStill = true; island.awake = false;
for ( auto* b : island.bodies ) { for ( auto idx : island.indices ) {
auto& body = *b; auto& body = *bodies[idx];
if ( !body.activity.awake ) continue; if ( !body.activity.awake ) continue;
float linSpeed = uf::vector::norm( body.velocity ); float linSpeed = uf::vector::norm( body.velocity );
@ -773,23 +808,18 @@ namespace {
body.activity.sleepTimer += dt; body.activity.sleepTimer += dt;
} else { } else {
body.activity.sleepTimer = 0.0f; body.activity.sleepTimer = 0.0f;
allStill = false; island.awake = true;
} }
if ( body.activity.sleepTimer < pod::Activity::sleepThreshold ) { if ( body.activity.sleepTimer < pod::Activity::sleepThreshold ) {
allStill = false; island.awake = true;
} }
} }
// put entire island to sleep // update bodies within island
if ( allStill ) { for ( auto idx : island.indices )
island.awake = false; (island.awake ? ::wakeBody : ::sleepBody)( *bodies[idx] );
for ( auto* b : island.bodies ) ::sleepBody( *b );
} return island.awake;
// at least one body is awake
else {
for ( auto* b : island.bodies ) ::wakeBody( *b );
island.awake = true;
}
} }
} }

14
engine/src/utils/math/physics/capsule.inl
View File

@ -6,7 +6,7 @@ namespace {
auto [ B1, B2 ] = ::getCapsuleSegment( b ); auto [ B1, B2 ] = ::getCapsuleSegment( b );
auto [ pA, pB ] = ::closestSegmentSegment( A1, A2, B1, B2 ); auto [ pA, pB ] = ::closestSegmentSegment( A1, A2, B1, B2 );
float r = a.collider.u.capsule.radius + b.collider.u.capsule.radius; float r = a.collider.capsule.radius + b.collider.capsule.radius;
auto delta = pB - pA; auto delta = pB - pA;
float dist2 = uf::vector::dot(delta, delta); float dist2 = uf::vector::dot(delta, delta);
@ -26,7 +26,7 @@ namespace {
const auto& box = b; const auto& box = b;
auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); auto [ p1, p2 ] = ::getCapsuleSegment( capsule );
float r = capsule.collider.u.capsule.radius; float r = capsule.collider.capsule.radius;
auto closestPoint = ::closestPointSegmentAabb( p1, p2, box.bounds ); auto closestPoint = ::closestPointSegmentAabb( p1, p2, box.bounds );
auto closestSegment = ::closestPointOnSegment( closestPoint, p1, p2 ); auto closestSegment = ::closestPointOnSegment( closestPoint, p1, p2 );
@ -48,11 +48,11 @@ namespace {
const auto& capsule = a; const auto& capsule = a;
const auto& plane = b; const auto& plane = b;
const auto& normal = plane.collider.u.plane.normal; const auto& normal = plane.collider.plane.normal;
float o = plane.collider.u.plane.offset; float o = plane.collider.plane.offset;
auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); auto [ p1, p2 ] = ::getCapsuleSegment( capsule );
float r = capsule.collider.u.capsule.radius; float r = capsule.collider.capsule.radius;
// the "foot" is just whichever end of the capsule is closest to the normal // the "foot" is just whichever end of the capsule is closest to the normal
auto foot = ( uf::vector::dot( normal, p1 ) < uf::vector::dot( normal, p2 ) ) ? p1 : p2; auto foot = ( uf::vector::dot( normal, p1 ) < uf::vector::dot( normal, p2 ) ) ? p1 : p2;
@ -85,7 +85,7 @@ namespace {
auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); auto [ p1, p2 ] = ::getCapsuleSegment( capsule );
auto sphereCenter = ::getPosition( sphere ); auto sphereCenter = ::getPosition( sphere );
float r = capsule.collider.u.capsule.radius + sphere.collider.u.sphere.radius; float r = capsule.collider.capsule.radius + sphere.collider.sphere.radius;
// closest point on capsule segment to sphere center // closest point on capsule segment to sphere center
auto closest = ::closestPointOnSegment( sphereCenter, p1, p2 ); auto closest = ::closestPointOnSegment( sphereCenter, p1, p2 );
@ -97,7 +97,7 @@ namespace {
float penetration = r - dist; float penetration = r - dist;
auto normal = ::normalizeDelta( delta, dist, eps ); auto normal = ::normalizeDelta( delta, dist, eps );
auto contact = closest + normal * (capsule.collider.u.capsule.radius - penetration * 0.5f ); auto contact = closest + normal * (capsule.collider.capsule.radius - penetration * 0.5f );
manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
return true; return true;

12
engine/src/utils/math/physics/epa.inl
View File

@ -51,7 +51,7 @@ namespace {
} }
void expandPolytope( uf::stl::vector<pod::Face>& faces, const pod::SupportPoint& p ) { void expandPolytope( uf::stl::vector<pod::Face>& faces, const pod::SupportPoint& p ) {
uf::stl::vector<int> remove; uf::stl::vector<uint32_t> remove;
uf::stl::vector<std::pair<pod::SupportPoint, pod::SupportPoint>> borders; uf::stl::vector<std::pair<pod::SupportPoint, pod::SupportPoint>> borders;
remove.reserve( faces.size() ); remove.reserve( faces.size() );
@ -68,8 +68,8 @@ namespace {
} }
// remove visible faces // remove visible faces
for ( auto i = (int) remove.size() - 1; i >= 0; --i ) { for ( auto i = (int32_t) remove.size() - 1; i >= 0; --i ) {
int idx = remove[i]; auto idx = remove[i];
faces[idx] = faces.back(); faces[idx] = faces.back();
faces.pop_back(); faces.pop_back();
} }
@ -80,7 +80,7 @@ namespace {
} }
} }
pod::Contact epa( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Simplex& simplex, int maxIterations = 64, float eps = EPS(1.0e-4f) ) { pod::Contact epa( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Simplex& simplex, uint32_t maxIterations = 64, float eps = EPS(1.0e-4f) ) {
UF_ASSERT( ::isValidSimplex(simplex) ); UF_ASSERT( ::isValidSimplex(simplex) );
auto faces = ::initialPolytope(simplex); auto faces = ::initialPolytope(simplex);
@ -88,9 +88,9 @@ namespace {
for ( auto it = 0; it < maxIterations; ++it ) { for ( auto it = 0; it < maxIterations; ++it ) {
// find closest face // find closest face
int idx = -1; int32_t idx = -1;
float minDist = FLT_MAX; float minDist = FLT_MAX;
for ( int i = 0; i < faces.size(); ++i ) { for ( auto i = 0; i < faces.size(); ++i ) {
if ( faces[i].distance < minDist ) { if ( faces[i].distance < minDist ) {
minDist = faces[i].distance; minDist = faces[i].distance;
idx = i; idx = i;

10
engine/src/utils/math/physics/gjk.inl
View File

@ -3,7 +3,7 @@ namespace {
const auto transform = ::getTransform( body ); const auto transform = ::getTransform( body );
switch ( body.collider.type ) { switch ( body.collider.type ) {
case pod::ShapeType::SPHERE: { case pod::ShapeType::SPHERE: {
return transform.position + uf::vector::normalize( dir ) * body.collider.u.sphere.radius; return transform.position + uf::vector::normalize( dir ) * body.collider.sphere.radius;
} break; } break;
case pod::ShapeType::AABB: { case pod::ShapeType::AABB: {
return { return {
@ -14,13 +14,13 @@ namespace {
} break; } break;
case pod::ShapeType::CAPSULE: { case pod::ShapeType::CAPSULE: {
auto up = uf::quaternion::rotate( transform.orientation, pod::Vector3f{0,1,0} ); auto up = uf::quaternion::rotate( transform.orientation, pod::Vector3f{0,1,0} );
auto p1 = transform.position + up * body.collider.u.capsule.halfHeight; auto p1 = transform.position + up * body.collider.capsule.halfHeight;
auto p2 = transform.position - up * body.collider.u.capsule.halfHeight; auto p2 = transform.position - up * body.collider.capsule.halfHeight;
auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2; // get closest end auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2; // get closest end
return end + uf::vector::normalize( dir ) * body.collider.u.capsule.radius; return end + uf::vector::normalize( dir ) * body.collider.capsule.radius;
} }
case pod::ShapeType::TRIANGLE: { case pod::ShapeType::TRIANGLE: {
const auto& tri = body.collider.u.triangle; const auto& tri = body.collider.triangle;
float d0 = uf::vector::dot( tri.points[0], dir ); float d0 = uf::vector::dot( tri.points[0], dir );
float d1 = uf::vector::dot( tri.points[1], dir ); float d1 = uf::vector::dot( tri.points[1], dir );
float d2 = uf::vector::dot( tri.points[2], dir ); float d2 = uf::vector::dot( tri.points[2], dir );

2
engine/src/utils/math/physics/helpers.inl
View File

@ -43,7 +43,7 @@ namespace {
void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt ); void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
int flattenBVH( pod::BVH& bvh, int nodeID ); int32_t flattenBVH( pod::BVH& bvh, int32_t nodeID );
void traverseNodePair( const pod::BVH& bvh, int32_t leftID, int32_t rightID, pod::BVH::pairs_t& pairs ); void traverseNodePair( const pod::BVH& bvh, int32_t leftID, int32_t rightID, pod::BVH::pairs_t& pairs );
void traverseNodePair( const pod::BVH& a, int32_t nodeA, const pod::BVH& b, int32_t nodeB, pod::BVH::pairs_t& out ); void traverseNodePair( const pod::BVH& a, int32_t nodeA, const pod::BVH& b, int32_t nodeB, pod::BVH::pairs_t& out );

95
engine/src/utils/math/physics/impl.cpp
View File

@ -11,12 +11,12 @@ namespace {
bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK
bool fixedStep = true; bool fixedStep = true;
int substeps = 0; int32_t substeps = 0;
int reserveCount = 32; int32_t reserveCount = 32;
// increasing these make things lag // increasing these make things lag
int broadphaseBvhCapacity = 1; int32_t broadphaseBvhCapacity = 1;
int meshBvhCapacity = 1; int32_t meshBvhCapacity = 1;
bool flattenBvhBodies = false; // bugged bool flattenBvhBodies = false; // bugged
bool flattenBvhMeshes = true; bool flattenBvhMeshes = true;
@ -27,12 +27,12 @@ namespace {
bool useSplitBvhs = true; // currently bugged if enabled bool useSplitBvhs = true; // currently bugged if enabled
int solverIterations = 10; int32_t solverIterations = 10;
float baumgarteCorrectionPercent = 0.2f; float baumgarteCorrectionPercent = 0.2f;
float baumgarteCorrectionSlop = 0.01f; float baumgarteCorrectionSlop = 0.01f;
uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache; uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
int manifoldCacheLifetime = 6; int32_t manifoldCacheLifetime = 6;
uint32_t frameCounter = 0; uint32_t frameCounter = 0;
pod::BVH::UpdatePolicy bvhUpdatePolicy = { pod::BVH::UpdatePolicy bvhUpdatePolicy = {
@ -111,7 +111,7 @@ void uf::physics::impl::terminate( pod::World& world ) {
} }
// Implementation // Implementation
void uf::physics::impl::substep( pod::World& world, float dt, int substeps ) { void uf::physics::impl::substep( pod::World& world, float dt, int32_t substeps ) {
float h = dt / substeps; float h = dt / substeps;
for ( auto i=0; i < substeps; ++i) { for ( auto i=0; i < substeps; ++i) {
uf::physics::impl::step( world, h ); uf::physics::impl::step( world, h );
@ -131,7 +131,7 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
::integrate( *body, dt ); ::integrate( *body, dt );
} }
// rebuild static bvh if diry // rebuild static bvh if dirty
if ( staticBvh.dirty && ::useSplitBvhs ) { if ( staticBvh.dirty && ::useSplitBvhs ) {
::buildBroadphaseBVH( staticBvh, bodies, ::broadphaseBvhCapacity, ::useSplitBvhs, true ); // (re)build ::buildBroadphaseBVH( staticBvh, bodies, ::broadphaseBvhCapacity, ::useSplitBvhs, true ); // (re)build
} }
@ -144,9 +144,9 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
::refitBVH( dynamicBvh, bodies ); // refit ::refitBVH( dynamicBvh, bodies ); // refit
} break; } break;
case pod::BVH::UpdatePolicy::Decision::NONE: case pod::BVH::UpdatePolicy::Decision::NONE:
default: default: {
// no-op // no-op
break; } break;
} }
// query for overlaps // query for overlaps
@ -156,33 +156,34 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
::queryOverlaps( dynamicBvh, staticBvh, pairs ); ::queryOverlaps( dynamicBvh, staticBvh, pairs );
} }
// build islands // build islands from overlaps
uf::stl::vector<pod::Island> islands; uf::stl::vector<pod::Island> islands;
::buildIslands( pairs, bodies, islands ); ::buildIslands( pairs, bodies, islands );
// update sleep state per island // iterate islands
for ( auto& island : islands ) ::updateIsland( island, dt ); #pragma omp parallel for schedule(dynamic)
for ( auto& island : islands ) {
uf::stl::vector<pod::Manifold> manifolds;
manifolds.reserve(::reserveCount);
// iterate overlaps // sleeping island, skip
uf::stl::vector<pod::Manifold> manifolds; if ( !::updateIsland( island, bodies, dt ) ) continue;
manifolds.reserve(::reserveCount); // iterate overlap pairs
for ( auto& [ ia, ib ] : island.pairs ) {
auto& a = *bodies[ia];
auto& b = *bodies[ib];
for ( auto [ia, ib] : pairs ) { pod::Manifold manifold;
auto& a = *bodies[ia]; // did not collide
auto& b = *bodies[ib]; if ( !::generateContacts( a, b, manifold, dt ) ) continue;
// could be also pruned in the broadphase, but traversal needs to be agnostic between a BVH for bodies or a BVH for triangles
if ( !::shouldCollide( a, b ) ) continue;
pod::Manifold manifold;
if ( ::generateContacts( a, b, manifold, dt ) ) {
// bodies with meshes already reorient the normal to the triangle's center // bodies with meshes already reorient the normal to the triangle's center
// do not do it for meshes because it'll reorient to the mesh's origin // do not do it for meshes because it'll reorient to the mesh's origin
if ( a.collider.type != pod::ShapeType::MESH && b.collider.type != pod::ShapeType::MESH ) { if ( a.collider.type != pod::ShapeType::MESH && b.collider.type != pod::ShapeType::MESH ) {
for ( auto& c : manifold.points ) c.normal = ::orientNormalToAB( a, b, c.normal ); for ( auto& c : manifold.points ) c.normal = ::orientNormalToAB( a, b, c.normal );
} }
// retrieve accumulated impulses // retrieve accumulated impulses
if ( ::warmupSolver ) ::retrieveContacts( manifold, ::manifoldsCache[::makePairKey( a, b )] ); if ( ::warmupSolver ) ::retrieveContacts( manifold, ::manifoldsCache[::makePairKey( a, b )] );
// merge similar contacts from a mesh to ensure continuity // merge similar contacts from a mesh to ensure continuity
if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) { if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) {
::mergeContacts( manifold ); ::mergeContacts( manifold );
@ -198,19 +199,13 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
// store manifold // store manifold
manifolds.emplace_back(manifold); manifolds.emplace_back(manifold);
} }
}
// pass manifolds to solver // pass manifolds to solver
::solveContacts( manifolds, dt ); ::solveContacts( manifolds, dt );
// do position correction // do position correction
::solvePositions( manifolds, dt ); ::solvePositions( manifolds, dt );
// cache manifold positions
if ( ::warmupSolver ) ::storeManifolds( manifolds, ::manifoldsCache ); if ( ::warmupSolver ) ::storeManifolds( manifolds, ::manifoldsCache );
// recompute bounds for further queries
for ( auto* body : bodies ) {
if ( body->isStatic ) continue;
body->bounds = ::computeAABB( *body );
} }
} }
@ -265,7 +260,7 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) {
switch ( body.collider.type ) { switch ( body.collider.type ) {
case pod::ShapeType::AABB: { case pod::ShapeType::AABB: {
pod::Vector3f extents = (body.collider.u.aabb.max - body.collider.u.aabb.min); pod::Vector3f extents = (body.collider.aabb.max - body.collider.aabb.min);
extents *= extents; // square it; extents *= extents; // square it;
body.inertiaTensor = extents * (body.mass / 12.0f); body.inertiaTensor = extents * (body.mass / 12.0f);
@ -274,14 +269,14 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) {
body.inverseInertiaTensor = { 1.0f / body.inertiaTensor.x, 1.0f / body.inertiaTensor.y, 1.0f / body.inertiaTensor.z }; body.inverseInertiaTensor = { 1.0f / body.inertiaTensor.x, 1.0f / body.inertiaTensor.y, 1.0f / body.inertiaTensor.z };
} break; } break;
case pod::ShapeType::SPHERE: { case pod::ShapeType::SPHERE: {
float I = 0.4f * body.mass * body.collider.u.sphere.radius * body.collider.u.sphere.radius; float I = 0.4f * body.mass * body.collider.sphere.radius * body.collider.sphere.radius;
float invI = 1.0f / I; float invI = 1.0f / I;
body.inertiaTensor = { I, I, I }; body.inertiaTensor = { I, I, I };
body.inverseInertiaTensor = { invI, invI, invI }; body.inverseInertiaTensor = { invI, invI, invI };
} break; } break;
case pod::ShapeType::CAPSULE: { case pod::ShapeType::CAPSULE: {
float r = body.collider.u.capsule.radius; float r = body.collider.capsule.radius;
float h = body.collider.u.capsule.halfHeight * 2.0f; // full cyl height float h = body.collider.capsule.halfHeight * 2.0f; // full cyl height
float m = body.mass; float m = body.mass;
float Ixx = 0.25f * m * r * r + (1.0f/12.0f) * m * h * h; float Ixx = 0.25f * m * r * r + (1.0f/12.0f) * m * h * h;
@ -359,7 +354,7 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) { pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::impl::create( world, object, mass, offset ); auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::AABB; body.collider.type = pod::ShapeType::AABB;
body.collider.u.aabb = aabb; body.collider.aabb = aabb;
body.bounds = ::computeAABB( body ); body.bounds = ::computeAABB( body );
uf::physics::impl::updateInertia( body ); uf::physics::impl::updateInertia( body );
return body; return body;
@ -367,7 +362,7 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) { pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::impl::create( world, object, mass, offset ); auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::SPHERE; body.collider.type = pod::ShapeType::SPHERE;
body.collider.u.sphere = sphere; body.collider.sphere = sphere;
body.bounds = ::computeAABB( body ); body.bounds = ::computeAABB( body );
uf::physics::impl::updateInertia( body ); uf::physics::impl::updateInertia( body );
return body; return body;
@ -375,7 +370,7 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) { pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::impl::create( world, object, mass, offset ); auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::PLANE; body.collider.type = pod::ShapeType::PLANE;
body.collider.u.plane = plane; body.collider.plane = plane;
body.bounds = ::computeAABB( body ); body.bounds = ::computeAABB( body );
uf::physics::impl::updateInertia( body ); uf::physics::impl::updateInertia( body );
return body; return body;
@ -383,7 +378,7 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) { pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::impl::create( world, object, mass, offset ); auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::CAPSULE; body.collider.type = pod::ShapeType::CAPSULE;
body.collider.u.capsule = capsule; body.collider.capsule = capsule;
body.bounds = ::computeAABB( body ); body.bounds = ::computeAABB( body );
uf::physics::impl::updateInertia( body ); uf::physics::impl::updateInertia( body );
return body; return body;
@ -391,7 +386,7 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::TriangleWithNormal& tri, float mass, const pod::Vector3f& offset ) { pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::TriangleWithNormal& tri, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::impl::create( world, object, mass, offset ); auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::TRIANGLE; body.collider.type = pod::ShapeType::TRIANGLE;
body.collider.u.triangle = tri; body.collider.triangle = tri;
body.bounds = ::computeAABB( body ); body.bounds = ::computeAABB( body );
uf::physics::impl::updateInertia( body ); uf::physics::impl::updateInertia( body );
return body; return body;
@ -399,10 +394,10 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset ) { pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::impl::create( world, object, mass, offset ); auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::MESH; body.collider.type = pod::ShapeType::MESH;
body.collider.u.mesh.mesh = &mesh; body.collider.mesh.mesh = &mesh;
body.collider.u.mesh.bvh = new pod::BVH; body.collider.mesh.bvh = new pod::BVH;
auto& bvh = *body.collider.u.mesh.bvh; auto& bvh = *body.collider.mesh.bvh;
::buildMeshBVH( bvh, mesh, ::meshBvhCapacity ); ::buildMeshBVH( bvh, mesh, ::meshBvhCapacity );
body.bounds = ::computeAABB( body ); body.bounds = ::computeAABB( body );
@ -461,7 +456,7 @@ void uf::physics::impl::destroy( pod::PhysicsBody& body ) {
// remove any pointered collider data // remove any pointered collider data
if ( body.collider.type == pod::ShapeType::MESH ) { if ( body.collider.type == pod::ShapeType::MESH ) {
if ( body.collider.u.mesh.bvh ) delete body.collider.u.mesh.bvh; if ( body.collider.mesh.bvh ) delete body.collider.mesh.bvh;
} }
} }

6
engine/src/utils/math/physics/integration.inl
View File

@ -131,7 +131,7 @@ namespace {
result.emplace_back(c); result.emplace_back(c);
} else { } else {
// Replace weakest if this one is stronger // Replace weakest if this one is stronger
int weakest = 0; auto weakest = 0;
for ( auto i = 1; i < 4; i++ ) { for ( auto i = 1; i < 4; i++ ) {
if ( result[i].penetration < result[weakest].penetration ) weakest = i; if ( result[i].penetration < result[weakest].penetration ) weakest = i;
} }
@ -264,14 +264,10 @@ namespace {
uf::transform::rotate( *body.transform/*.reference*/, dq ); uf::transform::rotate( *body.transform/*.reference*/, dq );
} }
// update AABB
body.bounds = ::computeAABB( body );
// reset accumulators // reset accumulators
body.forceAccumulator = {}; body.forceAccumulator = {};
body.torqueAccumulator = {}; body.torqueAccumulator = {};
// apply rolling resistance // apply rolling resistance
::applyRollingResistance( body, dt ); ::applyRollingResistance( body, dt );

24
engine/src/utils/math/physics/mesh.inl
View File

@ -19,8 +19,8 @@ namespace {
const auto& mesh = a; const auto& mesh = a;
const auto& aabb = b; const auto& aabb = b;
const auto& bvh = *mesh.collider.u.mesh.bvh; const auto& bvh = *mesh.collider.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& meshData = *mesh.collider.mesh.mesh;
// transform to local space for BVH query // transform to local space for BVH query
auto bounds = ::transformAabbToLocal( aabb.bounds, ::getTransform( mesh ) ); auto bounds = ::transformAabbToLocal( aabb.bounds, ::getTransform( mesh ) );
@ -42,8 +42,8 @@ namespace {
const auto& mesh = a; const auto& mesh = a;
const auto& sphere = b; const auto& sphere = b;
const auto& bvh = *mesh.collider.u.mesh.bvh; const auto& bvh = *mesh.collider.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& meshData = *mesh.collider.mesh.mesh;
// transform to local space for BVH query // transform to local space for BVH query
auto bounds = ::transformAabbToLocal( sphere.bounds, ::getTransform( mesh ) ); auto bounds = ::transformAabbToLocal( sphere.bounds, ::getTransform( mesh ) );
@ -67,8 +67,8 @@ namespace {
const auto& mesh = a; const auto& mesh = a;
const auto& plane = b; const auto& plane = b;
const auto& bvh = *mesh.collider.u.mesh.bvh; const auto& bvh = *mesh.collider.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& meshData = *mesh.collider.mesh.mesh;
// transform to local space for BVH query // transform to local space for BVH query
auto bounds = ::transformAabbToLocal( plane.bounds, ::getTransform( mesh ) ); auto bounds = ::transformAabbToLocal( plane.bounds, ::getTransform( mesh ) );
@ -91,8 +91,8 @@ namespace {
const auto& mesh = a; const auto& mesh = a;
const auto& capsule = b; const auto& capsule = b;
const auto& bvh = *mesh.collider.u.mesh.bvh; const auto& bvh = *mesh.collider.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& meshData = *mesh.collider.mesh.mesh;
// transform to local space for BVH query // transform to local space for BVH query
auto bounds = ::transformAabbToLocal( capsule.bounds, ::getTransform( mesh ) ); auto bounds = ::transformAabbToLocal( capsule.bounds, ::getTransform( mesh ) );
@ -113,11 +113,11 @@ namespace {
bool meshMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { bool meshMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) {
ASSERT_COLLIDER_TYPES( MESH, MESH ); ASSERT_COLLIDER_TYPES( MESH, MESH );
const auto& bvhA = *a.collider.u.mesh.bvh; const auto& bvhA = *a.collider.mesh.bvh;
const auto& meshA = *a.collider.u.mesh.mesh; const auto& meshA = *a.collider.mesh.mesh;
const auto& meshB = *b.collider.u.mesh.mesh; const auto& meshB = *b.collider.mesh.mesh;
const auto& bvhB = *b.collider.u.mesh.bvh; const auto& bvhB = *b.collider.mesh.bvh;
// compute overlaps between one BVH and another BVH // compute overlaps between one BVH and another BVH
pod::BVH::pairs_t pairs; pod::BVH::pairs_t pairs;

10
engine/src/utils/math/physics/plane.inl
View File

@ -5,8 +5,8 @@ namespace {
const auto& plane = a; const auto& plane = a;
const auto& aabb = b; const auto& aabb = b;
auto normal = uf::vector::normalize( plane.collider.u.plane.normal ); auto normal = uf::vector::normalize( plane.collider.plane.normal );
float offset = plane.collider.u.plane.offset; float offset = plane.collider.plane.offset;
auto center = ::aabbCenter( aabb.bounds ); // center auto center = ::aabbCenter( aabb.bounds ); // center
auto extent = ::aabbExtent( aabb.bounds ); // half extents auto extent = ::aabbExtent( aabb.bounds ); // half extents
@ -27,11 +27,11 @@ namespace {
const auto& plane = a; const auto& plane = a;
const auto& sphere = b; const auto& sphere = b;
auto& normal = plane.collider.u.plane.normal; auto& normal = plane.collider.plane.normal;
float offset = plane.collider.u.plane.offset; float offset = plane.collider.plane.offset;
auto center = ::getPosition( sphere ); auto center = ::getPosition( sphere );
float r = sphere.collider.u.sphere.radius; float r = sphere.collider.sphere.radius;
float dist = uf::vector::dot( normal, center ) - offset; float dist = uf::vector::dot( normal, center ) - offset;
if ( dist > r ) return false; if ( dist > r ) return false;

12
engine/src/utils/math/physics/ray.inl
View File

@ -76,7 +76,7 @@ namespace {
} }
bool raySphere( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) { bool raySphere( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) {
auto center = ::getPosition(body); auto center = ::getPosition(body);
float r = body.collider.u.sphere.radius; float r = body.collider.sphere.radius;
// vector from sphere center to ray origin // vector from sphere center to ray origin
auto oc = ray.origin - center; auto oc = ray.origin - center;
@ -109,8 +109,8 @@ namespace {
return true; return true;
} }
bool rayPlane( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit, float eps = EPS(1e-6f) ) { bool rayPlane( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit, float eps = EPS(1e-6f) ) {
auto& normal = body.collider.u.plane.normal; auto& normal = body.collider.plane.normal;
float offset = body.collider.u.plane.offset; float offset = body.collider.plane.offset;
float denom = uf::vector::dot( normal, ray.direction ); float denom = uf::vector::dot( normal, ray.direction );
if ( fabs(denom) < eps ) return false; // parallel if ( fabs(denom) < eps ) return false; // parallel
@ -129,7 +129,7 @@ namespace {
} }
bool rayCapsule( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) { bool rayCapsule( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) {
auto [ p1, p2 ] = ::getCapsuleSegment( body ); auto [ p1, p2 ] = ::getCapsuleSegment( body );
float r = body.collider.u.capsule.radius; float r = body.collider.capsule.radius;
auto d = p2 - p1; // segment direction auto d = p2 - p1; // segment direction
auto m = ray.origin - p1; auto m = ray.origin - p1;
@ -193,8 +193,8 @@ namespace {
} }
bool rayMesh( const pod::Ray& r, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) { bool rayMesh( const pod::Ray& r, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) {
const auto& meshData = *body.collider.u.mesh.mesh; const auto& meshData = *body.collider.mesh.mesh;
const auto& bvh = *body.collider.u.mesh.bvh; const auto& bvh = *body.collider.mesh.bvh;
const auto transform = ::getTransform( body ); const auto transform = ::getTransform( body );

17
engine/src/utils/math/physics/solvers.inl
View File

@ -87,11 +87,11 @@ namespace {
auto pA = ::getPosition( a, true ); auto pA = ::getPosition( a, true );
auto pB = ::getPosition( b, true ); auto pB = ::getPosition( b, true );
for (int i = 0; i < N; i++) { for ( auto i = 0; i < N; i++ ) {
pod::Vector3f rA_i = manifold.points[i].point - pA; pod::Vector3f rA_i = manifold.points[i].point - pA;
pod::Vector3f rB_i = manifold.points[i].point - pB; pod::Vector3f rB_i = manifold.points[i].point - pB;
for (int j = 0; j < N; j++) { for ( auto j = 0; j < N; j++ ) {
pod::Vector3f rA_j = manifold.points[j].point - pA; pod::Vector3f rA_j = manifold.points[j].point - pA;
pod::Vector3f rB_j = manifold.points[j].point - pB; pod::Vector3f rB_j = manifold.points[j].point - pB;
@ -189,7 +189,7 @@ namespace {
}; };
void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
const int count = std::min( (int)manifold.points.size(), 4 ); const uint32_t count = std::min( (uint32_t) manifold.points.size(), (uint32_t) 4 );
// precompute contact caches // precompute contact caches
::ContactCache cache[4]; ::ContactCache cache[4];
@ -288,12 +288,19 @@ namespace {
for ( auto i = 0; i < ::solverIterations; ++i ) for ( auto& manifold : manifolds ) ::resolveManifold( *manifold.a, *manifold.b, manifold, dt ); for ( auto i = 0; i < ::solverIterations; ++i ) for ( auto& manifold : manifolds ) ::resolveManifold( *manifold.a, *manifold.b, manifold, dt );
} }
void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, int iterations = 4 ) { void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 2 ) {
for ( auto i = 0; i < iterations; ++i ) { for ( auto i = 0; i < iterations; ++i ) {
for ( auto& m : manifolds ) { for ( auto& m : manifolds ) {
pod::Contact s = {};
float weight = 0;
for ( auto& c : m.points ) { for ( auto& c : m.points ) {
::positionCorrection( *m.a, *m.b, c ); float w = std::max( c.penetration, 0.0f );
s.normal += c.normal * w;
s.penetration = std::max(s.penetration, c.penetration);
weight += w;
} }
s.normal = weight > 0.0f ? uf::vector::normalize(s.normal) : pod::Vector3f{0,1,0};
::positionCorrection( *m.a, *m.b, s );
} }
} }
} }

6
engine/src/utils/math/physics/sphere.inl
View File

@ -3,7 +3,7 @@ namespace {
ASSERT_COLLIDER_TYPES( SPHERE, SPHERE ); ASSERT_COLLIDER_TYPES( SPHERE, SPHERE );
auto delta = ::getPosition( b ) - ::getPosition( a ); auto delta = ::getPosition( b ) - ::getPosition( a );
float r = a.collider.u.sphere.radius + b.collider.u.sphere.radius; float r = a.collider.sphere.radius + b.collider.sphere.radius;
float dist2 = uf::vector::dot(delta, delta); float dist2 = uf::vector::dot(delta, delta);
if ( dist2 > r * r ) return false; if ( dist2 > r * r ) return false;
@ -11,7 +11,7 @@ namespace {
float penetration = r - dist; float penetration = r - dist;
auto normal = ::normalizeDelta( delta, dist, eps ); auto normal = ::normalizeDelta( delta, dist, eps );
auto contact = ::getPosition( a ) + (normal * a.collider.u.sphere.radius); auto contact = ::getPosition( a ) + (normal * a.collider.sphere.radius);
manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
return true; return true;
@ -23,7 +23,7 @@ namespace {
const auto& aabb = b; const auto& aabb = b;
auto center = ::getPosition( sphere ); auto center = ::getPosition( sphere );
float r = sphere.collider.u.sphere.radius; float r = sphere.collider.sphere.radius;
auto& bounds = aabb.bounds; auto& bounds = aabb.bounds;
auto closest = pod::Vector3f{ auto closest = pod::Vector3f{

22
engine/src/utils/math/physics/triangle.inl
View File

@ -133,7 +133,7 @@ namespace {
// clip edges of A against plane of B // clip edges of A against plane of B
const pod::Vector3f At[3] = { A.points[0], A.points[1], A.points[2] }; const pod::Vector3f At[3] = { A.points[0], A.points[1], A.points[2] };
for ( auto i = 0; i < 3; ++i ) { for ( auto i = 0; i < 3; ++i ) {
int j = ( i + 1 ) % 3; auto j = ( i + 1 ) % 3;
pod::Vector3f p; pod::Vector3f p;
if ( intersectSegmentPlane( At[i], At[j], nB, dB, p ) ) { if ( intersectSegmentPlane( At[i], At[j], nB, dB, p ) ) {
// check if intersection lies inside triangle B // check if intersection lies inside triangle B
@ -144,7 +144,7 @@ namespace {
// clip edges of B against plane of A // clip edges of B against plane of A
const pod::Vector3f Bt[3] = { B.points[0], B.points[1], B.points[2] }; const pod::Vector3f Bt[3] = { B.points[0], B.points[1], B.points[2] };
for ( auto i = 0; i < 3; ++i ) { for ( auto i = 0; i < 3; ++i ) {
int j = ( i + 1 ) % 3; auto j = ( i + 1 ) % 3;
pod::Vector3f p; pod::Vector3f p;
if ( intersectSegmentPlane( Bt[i], Bt[j], nA, dA, p ) ) { if ( intersectSegmentPlane( Bt[i], Bt[j], nA, dA, p ) ) {
if ( ::pointInTriangle( p, A ) ) checkAndPush(p); if ( ::pointInTriangle( p, A ) ) checkAndPush(p);
@ -264,7 +264,7 @@ namespace {
bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) { bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) {
const auto& sphere = body; const auto& sphere = body;
float r = sphere.collider.u.sphere.radius; float r = sphere.collider.sphere.radius;
auto center = ::getPosition( sphere ); auto center = ::getPosition( sphere );
auto closest = ::closestPointOnTriangle( center, tri.points[0], tri.points[1], tri.points[2] ); auto closest = ::closestPointOnTriangle( center, tri.points[0], tri.points[1], tri.points[2] );
@ -290,8 +290,8 @@ namespace {
// to-do: implement // to-do: implement
bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) { bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) {
const auto& plane = body; const auto& plane = body;
auto normal = plane.collider.u.plane.normal; auto normal = plane.collider.plane.normal;
float d = plane.collider.u.plane.offset; float d = plane.collider.plane.offset;
bool hit = false; bool hit = false;
pod::Vector3f dist; pod::Vector3f dist;
@ -335,7 +335,7 @@ namespace {
bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) { bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) {
const auto& capsule = body; const auto& capsule = body;
float r = capsule.collider.u.capsule.radius; float r = capsule.collider.capsule.radius;
auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); auto [ p1, p2 ] = ::getCapsuleSegment( capsule );
auto bounds = ::computeSegmentAABB( p1, p2, r ); auto bounds = ::computeSegmentAABB( p1, p2, r );
@ -363,22 +363,22 @@ namespace {
bool triangleTriangle( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { bool triangleTriangle( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) {
ASSERT_COLLIDER_TYPES( TRIANGLE, TRIANGLE ); ASSERT_COLLIDER_TYPES( TRIANGLE, TRIANGLE );
return ::triangleTriangle( a.collider.u.triangle, b.collider.u.triangle, manifold, eps ); return ::triangleTriangle( a.collider.triangle, b.collider.triangle, manifold, eps );
} }
bool triangleAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { bool triangleAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) {
ASSERT_COLLIDER_TYPES( TRIANGLE, AABB ); ASSERT_COLLIDER_TYPES( TRIANGLE, AABB );
return ::triangleAabb( a.collider.u.triangle, b, manifold, eps ); return ::triangleAabb( a.collider.triangle, b, manifold, eps );
} }
bool triangleSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { bool triangleSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) {
ASSERT_COLLIDER_TYPES( TRIANGLE, SPHERE ); ASSERT_COLLIDER_TYPES( TRIANGLE, SPHERE );
return ::triangleSphere( a.collider.u.triangle, b, manifold, eps ); return ::triangleSphere( a.collider.triangle, b, manifold, eps );
} }
bool trianglePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { bool trianglePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) {
ASSERT_COLLIDER_TYPES( TRIANGLE, PLANE ); ASSERT_COLLIDER_TYPES( TRIANGLE, PLANE );
return ::trianglePlane( a.collider.u.triangle, b, manifold, eps ); return ::trianglePlane( a.collider.triangle, b, manifold, eps );
} }
bool triangleCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { bool triangleCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) {
ASSERT_COLLIDER_TYPES( TRIANGLE, CAPSULE ); ASSERT_COLLIDER_TYPES( TRIANGLE, CAPSULE );
return ::triangleCapsule( a.collider.u.triangle, b, manifold, eps ); return ::triangleCapsule( a.collider.triangle, b, manifold, eps );
} }
} }