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

physics optimizations (lots...)

Browse Source
This commit is contained in:
ecker 2025-09-02 21:51:00 -05:00
parent a66f56ad9f
commit 593bdc93cd
19 changed files with 1054 additions and 307 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
bin/data/config.json
View File

@ -303,7 +303,7 @@
},
"reactphysics": {
"global storage": false,
"timescale": 0.01666666666,
"timescale": 0.01666666666, // 0.03333333333,
"interpolate": true,
"gravity": {
"mode": "default", // default / per-object / universal

1
bin/data/scenes/sourceengine/base_sourceengine.json
View File

@ -16,6 +16,7 @@
"unwrap mesh": true
},
"worldspawn_skybox": {
"physics": { "type": "mesh", "static": true, "mass": 0 },
"grid": { "size": [8,1,8], "epsilon": 0.001, "cleanup": true, "print": true, "clip": true },
"optimize meshlets": { "simplify": 0.125, "print": false },
"unwrap mesh": true

4
bin/data/scenes/sourceengine/sourceengine.json
View File

@ -1,9 +1,9 @@
{
// "import": "./rp_downtown_v2.json"
// "import": "./ss2_medsci1.json"
"import": "./ss2_medsci1.json"
// "import": "./test_grid.json"
// "import": "./sh2_mcdonalds.json"
// "import": "./animal_crossing.json"
"import": "./mds_mcdonalds.json"
// "import": "./mds_mcdonalds.json"
// "import": "./gm_construct.json"
}

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

@ -43,16 +43,42 @@ namespace pod {
};
struct BVH {
typedef uf::stl::vector<std::pair<int,int>> pair_t;
typedef std::pair<int32_t,int32_t> pair_t;
typedef uf::stl::vector<pair_t> pairs_t;
struct Node {
pod::AABB bounds = {};
int left = -1;
int right = -1;
int start = 0;
int count = 0;
int32_t left = -1;
int32_t right = -1;
int32_t start = 0;
int32_t count = 0;
bool asleep = false;
};
uf::stl::vector<size_t> indices;
struct FlatNode {
pod::AABB bounds = {};
int32_t start = -1;
int32_t count = -1;
int32_t skipIndex = -1;
bool asleep = false;
};
struct UpdatePolicy {
enum class Decision {
NONE, // do nothing
REFIT, // refit bounds
REBUILD // rebuild from scratch
};
float displacementThreshold = 0.25f; // 25% of AABB size
float overlapThreshold = 2.0f; // 2x growth in root surface area
float dirtyRatioThreshold = 0.3f; // 30% dirty bodies
int maxFramesBeforeRebuild = 60; // force rebuild every 60 frames
};
bool dirty = false;
uf::stl::vector<uint32_t> indices;
uf::stl::vector<pod::BVH::Node> nodes;
uf::stl::vector<pod::BVH::FlatNode> flattened;
};
struct MeshBVH {
@ -109,6 +135,15 @@ namespace pod {
float dynamicFriction = 0.3f;
};
struct Activity {
bool awake = true;
float sleepTimer = 0.0f;
int32_t islandID = -1;
static constexpr float sleepThreshold = 0.5f; // seconds
static constexpr float linearSleepEpsilon = 0.01f; // m/s
static constexpr float angularSleepEpsilon = 0.01f; // rad/s
};
struct World; // forward declare
struct PhysicsBody {
@ -138,6 +173,7 @@ namespace pod {
pod::AABB bounds;
pod::Collider collider;
pod::PhysicsMaterial material;
pod::Activity activity;
};
struct Contact {
@ -165,6 +201,11 @@ namespace pod {
pod::Contact contact = { pod::Vector3f{}, pod::Vector3f{}, FLT_MAX };
};
struct Island {
bool awake = true;
uf::stl::vector<pod::PhysicsBody*> bodies;
};
struct World {
uf::stl::vector<pod::PhysicsBody*> bodies;

3
engine/inc/uf/utils/math/shapes.h
View File

@ -33,6 +33,9 @@ namespace pod {
};
struct TriangleWithNormal : Triangle {
pod::Vector3f normal;
};
struct TriangleWithNormals : Triangle {
pod::Vector3f normals[3];
};

6
engine/inc/uf/utils/mesh/mesh.h
View File

@ -371,11 +371,11 @@ namespace uf {
To* dstPtr = (To*) (dstBuffer.data());
if ( toEnum == uf::renderer::enums::Type::USHORT ) {
for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = uf::quant::quantize_f32u16(srcPtr[i]);
for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = (To) uf::quant::quantize_f32u16(srcPtr[i]);
} else if ( fromEnum == uf::renderer::enums::Type::USHORT ) {
for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = uf::quant::dequantize_u16f32(srcPtr[i]);
for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = (To) uf::quant::dequantize_u16f32(srcPtr[i]);
} else {
for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = srcPtr[i];
for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = (To) srcPtr[i];
}
srcBuffer.swap( dstBuffer );

2
engine/src/engine/graph/graph.cpp
View File

@ -1384,6 +1384,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
if ( ext::json::isNull( metadataJson["physics"]["min"] ) ) metadataJson["physics"]["min"] = uf::vector::encode( min );
if ( ext::json::isNull( metadataJson["physics"]["max"] ) ) metadataJson["physics"]["max"] = uf::vector::encode( max );
}
#if !UF_GRAPH_EXTENDED
if ( type == "mesh" ) {
auto& physicsBody = entity.getComponent<pod::PhysicsBody>();
float mass = phyziks["mass"].as(physicsBody.mass);
@ -1395,6 +1396,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
uf::physics::impl::create( entity.as<uf::Object>(), mesh, mass );
}
#endif
}
}
}

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

@ -12,6 +12,11 @@ namespace {
*/
}
inline float aabbSurfaceArea(const pod::AABB& aabb) {
auto d = uf::vector::max( ( aabb.max - aabb.min ), pod::Vector3f{} );
return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
}
pod::AABB computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) {
return {
uf::vector::min( p1, p2 ) - r,
@ -205,14 +210,14 @@ namespace {
if ( !::aabbOverlap( A, B ) ) return false;
// Calculate overlap extents
// calculate overlap extents
float overlaps[3] = {
std::min(A.max.x, B.max.x) - std::max(A.min.x, B.min.x),
std::min(A.max.y, B.max.y) - std::max(A.min.y, B.min.y),
std::min(A.max.z, B.max.z) - std::max(A.min.z, B.min.z)
};
// Determine collision axis = smallest overlap
// determine collision axis = smallest overlap
int axis = -1;
float minOverlap = FLT_MAX;
for (int i = 0; i < 3; ++i) {
@ -226,7 +231,7 @@ namespace {
pod::Vector3f normal{0,0,0};
normal[axis] = (delta[axis] < 0 ? -1.0f : 1.0f);
// Build manifold contacts: overlap region corners on the separating axis
// build manifold contacts: overlap region corners on the separating axis
float xMin = std::max(A.min.x, B.min.x);
float xMax = std::min(A.max.x, B.max.x);
float yMin = std::max(A.min.y, B.min.y);
@ -234,20 +239,20 @@ namespace {
float zMin = std::max(A.min.z, B.min.z);
float zMax = std::min(A.max.z, B.max.z);
// On chosen axis, clamp to overlapped rectangle -> 4 potential points
if (axis == 0) { // X-axis separation, so face-on overlap in YZ plane
// on chosen axis, clamp to overlapped rectangle -> 4 potential points
if (axis == 0) { // x-axis separation, so face-on overlap in YZ plane
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMax }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMax }, normal, minOverlap });
}
else if (axis == 1) { // Y-axis separation, overlap in XZ plane
else if (axis == 1) { // y-axis separation, overlap in XZ plane
manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap });
}
else if (axis == 2) { // Z-axis separation, overlap in XY plane
else if (axis == 2) { // z-axis separation, overlap in XY plane
manifold.points.emplace_back(pod::Contact{ { xMin, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMin, yMax, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });

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

@ -1,85 +1,5 @@
// BVH
namespace {
// collects a list of nodes that are overlapping with each other
void traverseNodePair( const pod::BVH& bvh, int indexA, int indexB, pod::BVH::pair_t& pairs ) {
const auto& nodeA = bvh.nodes[indexA];
const auto& nodeB = bvh.nodes[indexB];
if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return;
if ( nodeA.count > 0 && nodeB.count > 0 ) {
for ( auto i = 0; i < nodeA.count; ++i )
for ( auto j = 0; j < nodeB.count; ++j ) {
int indexA = bvh.indices[nodeA.start + i];
int indexB = bvh.indices[nodeB.start + j];
pairs.emplace_back(std::pair{indexA, indexB});
}
return;
}
if ( nodeA.count == 0 ) {
::traverseNodePair( bvh, nodeA.left, indexB, pairs );
::traverseNodePair( bvh, nodeA.right, indexB, pairs );
} else if ( nodeB.count == 0 ) {
::traverseNodePair( bvh, indexA, nodeB.left, pairs );
::traverseNodePair( bvh, indexA, nodeB.right, pairs );
}
}
// 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 indexA, const pod::BVH& bvhB, int indexB, pod::BVH::pair_t& pairs ) {
const auto& nodeA = bvhA.nodes[indexA];
const auto& nodeB = bvhB.nodes[indexB];
if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return;
if ( nodeA.count > 0 && nodeB.count > 0 ) {
for ( auto i = 0; i < nodeA.count; i++ ) {
for ( auto j = 0; j < nodeB.count; j++ ) {
auto indexA = bvhA.indices[nodeA.start+i];
auto indexB = bvhB.indices[nodeB.start+j];
pairs.emplace_back(std::pair{indexA, indexB});
}
}
return;
}
if ( nodeA.count == 0 ) {
::traverseNodePair( bvhA, nodeA.left, bvhB , indexB, pairs );
::traverseNodePair( bvhA, nodeA.right, bvhB , indexB, pairs );
} else if ( nodeB.count == 0 ) {
::traverseNodePair( bvhA, indexA, bvhB, nodeB.left, pairs );
::traverseNodePair( bvhA, indexA, bvhB, nodeB.right, pairs );
}
}
void traverseBVH( const pod::BVH& bvh, int nodeIdx, pod::BVH::pair_t& pairs ) {
const auto& node = bvh.nodes[nodeIdx];
if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i ) {
for ( auto j = i + 1; j < node.count; ++j ) {
int indexA = bvh.indices[node.start + i];
int indexB = bvh.indices[node.start + j];
if ( !::aabbOverlap( bvh.nodes[indexA].bounds, bvh.nodes[indexB].bounds ) ) {
continue;
}
pairs.emplace_back(std::pair{indexA, indexB});
}
}
return;
}
// recurse children
::traverseNodePair( bvh, node.left, node.right, pairs );
}
void queryOverlaps( const pod::BVH& bvh, pod::BVH::pair_t& outPairs ) {
if ( bvh.nodes.empty() ) return;
::traverseBVH( bvh, 0, outPairs );
}
int buildBVHNode( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, int start, int end, int capacity = 2 ) {
pod::BVH::Node node{};
node.left = -1;
@ -106,7 +26,7 @@ namespace {
int axis = (extent.x > extent.y && extent.x > extent.z) ? 0 : (extent.y > extent.z ? 1 : 2);
// sort indices by centroid along axis
std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](size_t a, size_t b) {
std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](uint32_t a, uint32_t b) {
float ca = ::aabbCenter( bounds[a] )[axis];
float cb = ::aabbCenter( bounds[b] )[axis];
return ca < cb;
@ -122,6 +42,125 @@ namespace {
return index;
}
int buildBVHNode_SAH( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, int start, int end, int capacity = 4 ) {
struct Bin {
pod::AABB bounds;
int count = 0;
};
pod::BVH::Node node{};
node.left = -1;
node.right = -1;
node.start = start;
node.count = 0;
node.bounds = bounds[bvh.indices[start]];
for ( auto i = start + 1; i < end; ++i ) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] );
int count = end - start;
if ( count <= capacity ) {
node.count = count;
int index = (int) bvh.nodes.size();
bvh.nodes.emplace_back(node);
return index;
}
constexpr int numBins = 16;
static thread_local Bin bins[numBins];
for ( auto i = 0; i < numBins; i++ ) bins[i] = {};
auto extent = node.bounds.max - node.bounds.min;
int bestAxis = -1, bestSplit = -1;
float bestCost = std::numeric_limits<float>::infinity();
for ( auto axis = 0; axis < 3; ++axis ) {
if ( extent[axis] < EPS(1e-6f) ) continue;
float minC = node.bounds.min[axis];
float maxC = node.bounds.max[axis];
float scale = (float) numBins / (maxC - minC);
for ( auto i = start; i < end; ++i ) {
int idx = bvh.indices[i];
float c = ::aabbCenter( bounds[idx] )[axis];
int binID = std::min(numBins - 1, (int)((c - minC) * scale));
bins[binID].count++;
bins[binID].bounds = ::mergeAabb( bins[binID].bounds, bounds[idx] );
}
pod::AABB leftBounds[numBins], rightBounds[numBins];
int leftCount[numBins] = {}, rightCount[numBins] = {};
pod::AABB acc;
int cnt = 0;
for ( auto i = 0; i < numBins; i++ ) {
if ( bins[i].count > 0 ) acc = (cnt == 0) ? bins[i].bounds : ::mergeAabb( acc, bins[i].bounds );
cnt += bins[i].count;
leftBounds[i] = acc;
leftCount[i] = cnt;
}
acc = {};
cnt = 0;
for ( auto i = numBins - 1; i >= 0; i-- ) {
if ( bins[i].count > 0 ) acc = (cnt == 0) ? bins[i].bounds : ::mergeAabb( acc, bins[i].bounds );
cnt += bins[i].count;
rightBounds[i] = acc;
rightCount[i] = cnt;
}
float parentArea = ::aabbSurfaceArea(node.bounds);
for ( auto i = 0; i < numBins - 1; i++ ) {
if ( leftCount[i] == 0 || rightCount[i + 1] == 0 ) continue;
float cost = 1.0f + (
( ::aabbSurfaceArea(leftBounds[i]) / parentArea ) * leftCount[i] +
( ::aabbSurfaceArea(rightBounds[i + 1]) / parentArea ) * rightCount[i + 1]
);
if ( cost < bestCost ) {
bestCost = cost;
bestAxis = axis;
bestSplit = i;
}
}
}
// fallback: no valid split → make leaf
if ( bestAxis == -1 ) {
node.count = count;
int index = (int) bvh.nodes.size();
bvh.nodes.emplace_back(node);
return index;
}
float minC = node.bounds.min[bestAxis];
float maxC = node.bounds.max[bestAxis];
float scale = (float) numBins / (maxC - minC);
auto midIt = std::partition( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](int idx) {
float c = ::aabbCenter( bounds[idx])[bestAxis ];
int binID = std::min(numBins - 1, (int)((c - minC) * scale));
return binID <= bestSplit;
});
int mid = (int) ( midIt - bvh.indices.begin() );
// if partition failed (all left or all right), force leaf
if ( mid == start || mid == end ) {
node.count = count;
int index = (int) bvh.nodes.size();
bvh.nodes.emplace_back(node);
return index;
}
int index = (int) bvh.nodes.size();
bvh.nodes.emplace_back(node);
node.left = ::buildBVHNode_SAH( bvh, bounds, start, mid, capacity );
node.right = ::buildBVHNode_SAH( bvh, bounds, mid, end, capacity );
bvh.nodes[index] = node;
return index;
}
void buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies, int capacity = 2 ) {
bvh.indices.clear();
bvh.nodes.clear();
@ -138,7 +177,13 @@ namespace {
}
// recursively build BVH from indices
::buildBVHNode( bvh, bounds, 0, bodies.size(), capacity );
if ( ::useBvhSahBodies ) ::buildBVHNode_SAH( bvh, bounds, 0, bodies.size(), capacity );
else ::buildBVHNode( bvh, bounds, 0, bodies.size(), capacity );
// flatten if requested
if ( ::flattenBvhBodies ) ::flattenBVH( bvh, 0 );
// mark as clean
bvh.dirty = false;
}
void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int capacity = 4 ) {
@ -165,62 +210,348 @@ namespace {
auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID );
auto triID = triIndexID + (view.index.first / 3);
bounds.emplace_back(aabb);
bvh.indices.emplace_back(triID); // triID => mesh.index.buffer[triID * 3];
if ( triID != bounds.size() ) UF_MSG_DEBUG("triID={}, bounds.size()={}", triID, bounds.size());
bounds.emplace_back( aabb );
bvh.indices.emplace_back( triID ); // triID => mesh.index.buffer[triID * 3];
}
}
// recursively build BVH from indices
if ( ::useBvhSahMeshes ) ::buildBVHNode_SAH( bvh, bounds, 0, triangles, capacity );
else ::buildBVHNode( bvh, bounds, 0, triangles, capacity );
// flatten if requested
if ( ::flattenBvhMeshes ) ::flattenBVH( bvh, 0 );
// mark as clean
bvh.dirty = false;
}
}
namespace {
pod::BVH::UpdatePolicy::Decision decideBVHUpdate( const pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies, const pod::BVH::UpdatePolicy& policy, int frameCounter ) {
if ( bvh.indices.empty() || bvh.nodes.empty() || bvh.dirty ) return pod::BVH::UpdatePolicy::Decision::REBUILD;
if ( bodies.empty() ) return pod::BVH::UpdatePolicy::Decision::NONE;
int dirtyCount = 0;
float oldRootArea = ::aabbSurfaceArea( bvh.nodes[0].bounds );
// check each body
for ( const auto* body : bodies ) {
pod::AABB newBounds = ::computeAABB(*body);
pod::AABB oldBounds = body->bounds;
// compute displacement relative to size
pod::Vector3f oldCenter = ( oldBounds.min + oldBounds.max ) * 0.5f;
pod::Vector3f newCenter = ( newBounds.min + newBounds.max ) * 0.5f;
float displacement = uf::vector::distance( newCenter, oldCenter );
pod::Vector3f extent = oldBounds.max - oldBounds.min;
float size = std::max({extent.x, extent.y, extent.z, 1e-6f});
if ( displacement > policy.displacementThreshold * size ) ++dirtyCount;
}
float dirtyRatio = (float) dirtyCount / (float) bodies.size();
// compute new root bounds
pod::AABB newRoot = bodies[0]->bounds;
for ( auto i = 1; i < bodies.size(); ++i ) {
newRoot = ::mergeAabb(newRoot, bodies[i]->bounds);
}
float newRootArea = ::aabbSurfaceArea( newRoot );
if ( dirtyRatio > policy.dirtyRatioThreshold || newRootArea > oldRootArea * policy.overlapThreshold || frameCounter % policy.maxFramesBeforeRebuild == 0 ) {
return pod::BVH::UpdatePolicy::Decision::REBUILD;
}
if ( dirtyCount > 0 ) return pod::BVH::UpdatePolicy::Decision::REFIT;
return pod::BVH::UpdatePolicy::Decision::NONE;
}
}
namespace {
void refitBVH( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds ) {
if ( bvh.nodes.empty() ) return;
// update leaf bounds
#pragma omp parallel for
for ( auto i = 0; i < bvh.nodes.size(); i++ ) {
auto& node = bvh.nodes[i];
if ( node.count > 0 ) {
// leaf node: recompute bounds from bodies
node.bounds = bounds[bvh.indices[node.start]];
for ( int j = 1; j < node.count; j++ ) {
node.bounds = ::mergeAabb(node.bounds, bounds[bvh.indices[node.start + j]] );
}
}
}
// update internal nodes bottom-up
for ( int i = (int) bvh.nodes.size() - 1; i >= 0; i-- ) {
auto& node = bvh.nodes[i];
// internal node
if ( node.count == 0 ) {
node.bounds = ::mergeAabb(bvh.nodes[node.left].bounds, bvh.nodes[node.right].bounds);
}
}
}
// avoids creating a vector for bounds
void refitBVH( pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies ) {
if ( bvh.nodes.empty() ) return;
// update leaf bounds
#pragma omp parallel for
for ( auto i = 0; i < bvh.nodes.size(); i++ ) {
auto& node = bvh.nodes[i];
if ( node.count > 0 ) {
// leaf node: recompute bounds from bodies
auto nodeID = bvh.indices[node.start];
node.bounds = ::computeAABB( *bodies[nodeID] );
node.asleep = !bodies[nodeID]->activity.awake;
for ( int j = 1; j < node.count; j++ ) {
auto bodyID = bvh.indices[node.start + j];
node.bounds = ::mergeAabb(node.bounds, ::computeAABB( *bodies[bodyID] ) );
node.asleep = node.asleep && !bodies[bodyID]->activity.awake;
}
}
}
// update internal nodes bottom-up
for ( int i = (int) bvh.nodes.size() - 1; i >= 0; i-- ) {
auto& node = bvh.nodes[i];
// internal node
if ( node.count == 0 ) {
const auto& leftNode = bvh.nodes[node.left];
const auto& rightNode = bvh.nodes[node.right];
node.bounds = ::mergeAabb(leftNode.bounds, rightNode.bounds);
node.asleep = leftNode.asleep && rightNode.asleep;
}
}
}
void refitBVH( pod::BVH& bvh, const uf::Mesh& mesh ) {
int triangles = mesh.index.count / 3;
uf::stl::vector<pod::AABB> bounds;
bounds.reserve( triangles );
auto views = mesh.makeViews({"position"});
UF_ASSERT( !views.empty() );
// populate initial indices and bounds
for ( auto& view : views ) {
auto& indices = view["index"];
auto& positions = view["position"];
auto tris = view.index.count / 3;
for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) {
auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID );
bounds.emplace_back(aabb);
}
}
UF_MSG_DEBUG("Built BVH with {} triangles from mesh with {} triangles ({} draw commands)", bounds.size(), triangles, views.size());
// recursively build BVH from indices
::buildBVHNode( bvh, bounds, 0, triangles, capacity );
::refitBVH( bvh, bounds );
}
}
namespace {
int flattenBVH( pod::BVH& bvh, int nodeID ) {
if ( nodeID == 0 ) bvh.flattened.reserve(bvh.nodes.size());
const auto& node = bvh.nodes[nodeID];
int flatID = (int) bvh.flattened.size();
bvh.flattened.emplace_back(); // placeholder
pod::BVH::FlatNode flat{};
flat.bounds = node.bounds;
flat.start = -1;
flat.count = -1;
flat.skipIndex = -1;
flat.asleep = node.asleep;
// leaf
if ( node.count > 0 ) {
flat.start = node.start;
flat.count = node.count;
flat.skipIndex = flatID + 1; // next node after this leaf
bvh.flattened[flatID] = flat;
return flatID + 1;
}
// internal
else {
flat.start = -1;
flat.count = 0;
int leftID = ::flattenBVH( bvh, node.left );
int rightID = ::flattenBVH( bvh, node.right );
flat.skipIndex = rightID; // skip entire subtree
bvh.flattened[flatID] = flat;
return rightID;
}
}
}
namespace {
// 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) {
const auto& nodeA = bvh.nodes[nodeAID];
const auto& nodeB = bvh.nodes[nodeBID];
if ( nodeA.asleep || nodeB.asleep || !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return;
if ( nodeA.count > 0 && nodeB.count > 0 ) {
for ( auto i = 0; i < nodeA.count; ++i ) {
for ( auto j = 0; j < nodeB.count; ++j ) {
int bodyA = bvh.indices[nodeA.start + i];
int bodyB = bvh.indices[nodeB.start + j];
if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
pairs.emplace_back(bodyA, bodyB);
}
}
return;
}
if ( nodeA.count == 0 ) {
::traverseNodePair( bvh, nodeA.left, nodeBID, pairs );
::traverseNodePair( bvh, nodeA.right, nodeBID, pairs );
}
if ( nodeB.count == 0 ) {
::traverseNodePair( bvh, nodeAID, nodeB.left, pairs );
::traverseNodePair( bvh, nodeAID, nodeB.right, pairs );
}
}
// 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 ) {
const auto& nodeA = bvhA.nodes[nodeAID];
const auto& nodeB = bvhB.nodes[nodeBID];
if ( nodeA.asleep || nodeB.asleep || !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return;
if ( nodeA.count > 0 && nodeB.count > 0 ) {
for ( auto i = 0; i < nodeA.count; ++i ) {
for ( auto j = 0; j < nodeB.count; ++j ) {
int bodyA = bvhA.indices[nodeA.start + i];
int bodyB = bvhB.indices[nodeB.start + j];
pairs.emplace_back(bodyA, bodyB);
}
}
return;
}
if ( nodeA.count == 0 ) {
::traverseNodePair( bvhA, nodeA.left, bvhB, nodeBID, pairs );
::traverseNodePair( bvhA, nodeA.right, bvhB, nodeBID, pairs );
}
if ( nodeB.count == 0 ) {
::traverseNodePair( bvhA, nodeAID, bvhB, nodeB.left, pairs );
::traverseNodePair( bvhA, nodeAID, bvhB, nodeB.right, pairs );
}
}
void traverseBVH( const pod::BVH& bvh, int nodeID, pod::BVH::pairs_t& pairs ) {
const auto& node = bvh.nodes[nodeID];
if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i ) {
for ( auto j = i + 1; j < node.count; ++j ) {
int bodyA = bvh.indices[node.start + i];
int bodyB = bvh.indices[node.start + j];
if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
pairs.emplace_back(bodyA, bodyB);
}
}
return;
}
::traverseNodePair( bvh, node.left, node.right, pairs );
}
void queryOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ) {
if ( !bvh.flattened.empty() ) return ::queryFlatOverlaps( bvh, outPairs );
if ( bvh.nodes.empty() ) return;
outPairs.reserve(::reserveCount);
::traverseBVH( bvh, 0, outPairs );
}
void queryOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ) {
if ( !bvhA.flattened.empty() && !bvhB.flattened.empty() ) return ::queryFlatOverlaps( bvhA, bvhB, outPairs );
if ( bvhA.nodes.empty() || bvhB.nodes.empty() ) return;
outPairs.reserve(::reserveCount);
::traverseNodePair(bvhA, 0, bvhB, 0, outPairs);
}
}
namespace {
// query a BVH with an AABB via a stack
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int>& indices ) {
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& outIndices ) {
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices );
outIndices.reserve(::reserveCount);
if ( bvh.nodes.empty() ) return;
uf::stl::stack<int> stack;
uf::stl::stack<int32_t> stack;
stack.push(0);
while ( !stack.empty() ) {
int idx = stack.top(); stack.pop();
auto& node = bvh.nodes[idx];
if ( !::aabbOverlap( bounds, node.bounds ) ) continue;
if ( node.asleep || !::aabbOverlap( bounds, node.bounds ) ) continue;
if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i) indices.emplace_back(bvh.indices[node.start + i]);
for ( auto i = 0; i < node.count; ++i) outIndices.emplace_back(bvh.indices[node.start + i]);
} else {
stack.push(node.left);
stack.push(node.right);
}
}
}
void queryBVH( const pod::BVH& bvh, const pod::PhysicsBody& body, uf::stl::vector<int>& indices ) {
return ::queryBVH( bvh, body.bounds, indices );
void queryBVH( const pod::BVH& bvh, const pod::PhysicsBody& body, uf::stl::vector<int32_t>& outIndices ) {
return ::queryBVH( bvh, body.bounds, outIndices );
}
// query a BVH with an AABB via recursion
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int>& indices, int nodeID ) {
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& outIndices, int nodeID ) {
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices );
if ( nodeID == 0 ) outIndices.reserve(::reserveCount);
const auto& node = bvh.nodes[nodeID];
if ( !::aabbOverlap( node.bounds, bounds ) ) return;
if ( node.asleep || !::aabbOverlap( node.bounds, bounds ) ) return;
if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i ) indices.emplace_back(bvh.indices[node.start + i]);
for ( auto i = 0; i < node.count; ++i ) outIndices.emplace_back(bvh.indices[node.start + i]);
return;
}
// recurse
::queryBVH( bvh, bounds, indices, node.left );
::queryBVH( bvh, bounds, indices, node.right );
::queryBVH( bvh, bounds, outIndices, node.left );
::queryBVH( bvh, bounds, outIndices, node.right );
}
// query a BVH with a ray via a stack
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int>& indices, float maxDist ) {
if ( bvh.nodes.empty() ) return;
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& outIndices, float maxDist ) {
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, ray, outIndices, maxDist );
uf::stl::stack<int> stack;
if ( bvh.nodes.empty() ) return;
outIndices.reserve(::reserveCount);
uf::stl::stack<int32_t> stack;
stack.push(0);
while ( !stack.empty() ) {
@ -228,11 +559,11 @@ namespace {
const auto& node = bvh.nodes[idx];
float tMin, tMax;
if ( !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) continue;
if ( node.asleep || !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) continue;
if ( tMin > maxDist ) continue;
if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i) indices.emplace_back(bvh.indices[node.start + i]);
for ( auto i = 0; i < node.count; ++i) outIndices.emplace_back(bvh.indices[node.start + i]);
} else {
stack.push(node.left);
stack.push(node.right);
@ -240,19 +571,212 @@ namespace {
}
}
// query a BVH with a ray via recursion
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int>& indices, int nodeID, float maxDist ) {
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& outIndices, int nodeID, float maxDist ) {
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, ray, outIndices, maxDist );
if ( nodeID == 0 ) outIndices.reserve(::reserveCount);
const auto& node = bvh.nodes[nodeID];
float tMin, tMax;
if ( !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) return;
if ( node.asleep || !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) return;
if ( tMin > maxDist ) return;
if ( node.count > 0 ) {
for ( auto i = 0; i < node.count; ++i ) indices.emplace_back(bvh.indices[node.start + i]);
for ( auto i = 0; i < node.count; ++i ) outIndices.emplace_back(bvh.indices[node.start + i]);
return;
}
// recurse
::queryBVH( bvh, ray, indices, node.left, maxDist );
::queryBVH( bvh, ray, indices, node.right, maxDist );
::queryBVH( bvh, ray, outIndices, node.left, maxDist );
::queryBVH( bvh, ray, outIndices, node.right, maxDist );
}
}
namespace {
void queryFlatOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ) {
auto& nodes = bvh.flattened;
auto& indices = bvh.indices;
outPairs.clear();
outPairs.reserve(::reserveCount);
for ( auto i = 0; i < (int) nodes.size(); ++i ) {
const auto& nodeA = nodes[i];
if ( nodeA.count <= 0 || nodeA.asleep ) continue;
for ( auto j = i + 1; j < (int) nodes.size(); ++j ) {
const auto& nodeB = nodes[j];
if ( nodeB.count <= 0 || nodeB.asleep ) continue;
if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) continue;
for ( auto ia = 0; ia < nodeA.count; ++ia ) {
for ( auto ib = 0; ib < nodeB.count; ++ib ) {
auto indexA = indices[nodeA.start + ia];
auto indexB = indices[nodeB.start + ib];
if ( indexA == indexB ) continue;
if ( indexA > indexB ) std::swap( indexA, indexB );
outPairs.emplace_back( indexA, indexB );
}
}
}
}
}
void queryFlatOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ) {
auto& nodesA = bvhA.flattened;
auto& indicesA = bvhA.indices;
auto& nodesB = bvhB.flattened;
auto& indicesB = bvhB.indices;
if ( nodesA.empty() || nodesB.empty() ) return;
outPairs.clear();
outPairs.reserve(::reserveCount);
for ( auto i = 0; i < (int) nodesA.size(); ++i ) {
const auto& nodeA = nodesA[i];
if ( nodeA.count <= 0 || nodeA.asleep ) continue;
for ( auto j = 0; j < (int) nodesB.size(); ++j ) {
const auto& nodeB = nodesB[j];
if ( nodeB.count <= 0 || nodeB.asleep ) continue;
if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) continue;
for ( auto ia = 0; ia < nodeA.count; ++ia ) {
for (auto ib = 0; ib < nodeB.count; ++ib ) {
auto indexA = indicesA[nodeA.start + ia];
auto indexB = indicesB[nodeB.start + ib];
outPairs.emplace_back( indexA, indexB );
}
}
}
}
}
void queryFlatBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& outIndices ) {
auto& nodes = bvh.flattened;
auto& indices = bvh.indices;
outIndices.reserve(::reserveCount);
int idx = 0;
while ( idx < (int) nodes.size() ) {
const auto& node = nodes[idx];
if ( !node.asleep && ::aabbOverlap( bounds, node.bounds ) ) {
// leaf
if ( node.count > 0 ) {
for ( int i = 0; i < node.count; ++i ) {
outIndices.emplace_back( indices[node.start + i] );
}
}
++idx;
} else {
// skip this subtree
idx = node.skipIndex;
}
}
}
void queryFlatBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& outIndices, float maxDist ) {
auto& nodes = bvh.flattened;
auto& indices = bvh.indices;
outIndices.reserve(::reserveCount);
int idx = 0;
while ( idx < (int) nodes.size() ) {
const auto& node = nodes[idx];
float tMin, tMax;
if ( !node.asleep && ::rayAabbIntersect( ray, node.bounds, tMin, tMax ) && tMin <= maxDist ) {
// leaf
if ( node.count > 0 ) {
for ( int i = 0; i < node.count; ++i ) {
outIndices.emplace_back( indices[node.start + i] );
}
}
++idx;
} else {
// skip this subtree
idx = node.skipIndex;
}
}
}
}
namespace {
void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands ) {
islands.reserve(::reserveCount);
int n = (int) bodies.size();
uf::stl::vector<int32_t> visited(n, -1);
for ( auto i = 0; i < n; i++ ) {
if ( visited[i] != -1 ) continue;
// new island
pod::Island island = {};
uf::stl::stack<int32_t> stack;
stack.push(i);
while ( !stack.empty() ) {
int idx = stack.top(); stack.pop();
if ( visited[idx] != -1 ) continue;
visited[idx] = (int) islands.size();
island.bodies.emplace_back( bodies[idx] );
// traverse neighbors
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 ) );
}
}
void updateIsland( pod::Island& island, float dt ) {
bool allStill = true;
for ( auto* b : island.bodies ) {
auto& body = *b;
if ( !body.activity.awake ) continue;
float linSpeed = uf::vector::norm( body.velocity );
float angSpeed = uf::vector::norm( body.angularVelocity );
if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon) {
body.activity.sleepTimer += dt;
} else {
body.activity.sleepTimer = 0.0f;
allStill = false;
}
if ( body.activity.sleepTimer < pod::Activity::sleepThreshold ) {
allStill = false;
}
}
// put entire island to sleep
if ( allStill ) {
island.awake = false;
for ( auto* b : island.bodies ) ::sleepBody( *b );
}
// at least one body is awake
else {
for ( auto* b : island.bodies ) ::wakeBody( *b );
island.awake = true;
}
}
}

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

@ -53,6 +53,9 @@ namespace {
void expandPolytope( uf::stl::vector<pod::Face>& faces, const pod::SupportPoint& p ) {
uf::stl::vector<int> remove;
uf::stl::vector<std::pair<pod::SupportPoint, pod::SupportPoint>> borders;
remove.reserve( faces.size() );
borders.reserve( faces.size() );
// find faces visible to point
for ( auto i = 0; i < faces.size(); ++i ) {

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

@ -21,13 +21,13 @@ namespace {
}
case pod::ShapeType::TRIANGLE: {
const auto& tri = body.collider.u.triangle;
float d0 = uf::vector::dot( tri.points[0], dir );
float d1 = uf::vector::dot( tri.points[1], dir );
float d2 = uf::vector::dot( tri.points[2], dir );
float d0 = uf::vector::dot( tri.points[0], dir );
float d1 = uf::vector::dot( tri.points[1], dir );
float d2 = uf::vector::dot( tri.points[2], dir );
if ( d0 > d1 && d0 > d2 ) return tri.points[0];
if ( d1 > d2 ) return tri.points[1];
return tri.points[2];
if ( d0 > d1 && d0 > d2 ) return tri.points[0];
if ( d1 > d2 ) return tri.points[1];
return tri.points[2];
} break;
default: {

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

@ -36,19 +36,32 @@ namespace {
bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
// ugh
pod::Vector3f aabbCenter( const pod::AABB& aabb );
bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) );
pod::AABB computeTriangleAABB( const pod::Triangle& tri );
void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
void traverseNodePair( const pod::BVH& bvh, int leftID, int rightID, pod::BVH::pair_t& pairs );
void traverseNodePair( const pod::BVH& a, int nodeA, const pod::BVH& b, int nodeB, pod::BVH::pair_t& out );
int flattenBVH( pod::BVH& bvh, int nodeID );
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 queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int>& indices );
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int>& indices, int nodeID );
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int>& indices, int nodeID, float maxDist = FLT_MAX );
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int>& indices, float maxDist = FLT_MAX );
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& indices );
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& indices, int32_t nodeID );
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& indices, float maxDist = FLT_MAX );
void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector<int32_t>& indices, int32_t nodeID, float maxDist = FLT_MAX );
void queryFlatBVH( const pod::BVH&, const pod::AABB& bounds, uf::stl::vector<int32_t>& out );
void queryFlatBVH( const pod::BVH&, const pod::Ray& ray, uf::stl::vector<int32_t>& out, float maxDist = FLT_MAX );
void queryOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs );
void queryOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs );
void queryFlatOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs );
void queryFlatOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs );
}
namespace {
@ -60,6 +73,40 @@ namespace {
return (idA << 32) ^ idB;
}
void deduplicatePairs( pod::BVH::pairs_t& pairs ) {
// should already be swapped
for (auto& [a, b] : pairs) if (a > b) std::swap(a, b);
std::sort(pairs.begin(), pairs.end());
pairs.erase(std::unique(pairs.begin(), pairs.end()), pairs.end());
}
// marks a body as asleep
void wakeBody( pod::PhysicsBody& body ) {
body.activity.awake = true;
body.activity.sleepTimer = 0.0f;
}
void sleepBody( pod::PhysicsBody& body ) {
body.activity.awake = false;
body.velocity = pod::Vector3f{};
body.angularVelocity = pod::Vector3f{};
}
void updateActivity( pod::PhysicsBody& body, float dt ) {
// already asleep
if ( !body.activity.awake ) return;
// check if body is moving
float linSpeed = uf::vector::norm( body.velocity );
float angSpeed = uf::vector::norm( body.angularVelocity );
// body is nearly still
if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon ) {
body.activity.sleepTimer += dt;
if ( body.activity.sleepTimer > pod::Activity::sleepThreshold ) ::sleepBody( body );
}
// body is moving, reset timer
else ::wakeBody( body );
}
// returns an absolute transform while also allowing offsetting the collision body
// to-do: find a succint way to explain this madness
pod::Transform<> getTransform( const pod::PhysicsBody& body ) {
@ -78,6 +125,7 @@ namespace {
return ( a.category & b.mask ) && ( b.category & a.mask );
}
bool shouldCollide( const pod::PhysicsBody& a, const pod::PhysicsBody& b ) {
// if ( a.isStatic && b.isStatic ) return false;
return ::shouldCollide( a.collider, b.collider );
}
@ -240,7 +288,7 @@ namespace {
}
pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) {
// Check if P in vertex region outside A
// check if P in vertex region outside A
pod::Vector3f ab = b - a;
pod::Vector3f ac = c - a;
pod::Vector3f ap = p - a;
@ -248,40 +296,40 @@ namespace {
float d2 = uf::vector::dot(ac, ap);
if (d1 <= 0 && d2 <= 0) return a;
// Check if P in vertex region outside B
// check if P in vertex region outside B
pod::Vector3f bp = p - b;
float d3 = uf::vector::dot(ab, bp);
float d4 = uf::vector::dot(ac, bp);
if (d3 >= 0 && d4 <= d3) return b;
// Check if P in edge region of AB, if so return projection on AB
// check if P in edge region of AB, if so return projection on AB
float vc = d1 * d4 - d3 * d2;
if (vc <= 0 && d1 >= 0 && d3 <= 0) {
float v = d1 / (d1 - d3);
return a + ab * v;
}
// Check vertex region outside C
// check vertex region outside C
pod::Vector3f cp = p - c;
float d5 = uf::vector::dot(ab, cp);
float d6 = uf::vector::dot(ac, cp);
if (d6 >= 0 && d5 <= d6) return c;
// Check edge region of AC
// check edge region of AC
float vb = d5 * d2 - d1 * d6;
if (vb <= 0 && d2 >= 0 && d6 <= 0) {
float w = d2 / (d2 - d6);
return a + ac * w;
}
// Check edge region of BC
// check edge region of BC
float va = d3 * d6 - d5 * d4;
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) {
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
return b + (c - b) * w;
}
// P inside face region. Return projection onto face
// p inside face region. Return projection onto face
float denom = 1.0f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;

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

@ -6,20 +6,39 @@
namespace {
bool warmupSolver = true;
bool blockContactSolver = false; // blockNxN solver is flawed
bool blockContactSolver = true;
bool psgContactSolver = true; // iterative solver is flawed
bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK
bool fixedStep = true;
int substeps = 4;
int substeps = 0;
int reserveCount = 32;
// increasing these make things lag
int broadphaseBvhCapacity = 1;
int meshBvhCapacity = 1;
int solverIterations = 5;
float baumgarteCorrectionPercent = 0.005f; // needs to be very small or the correction is too large
float baumgarteCorrectionSlop = 0.001f;
bool flattenBvhBodies = true;
bool flattenBvhMeshes = true;
// it actually seems slower to use these......
bool useBvhSahBodies = false;
bool useBvhSahMeshes = false;
int solverIterations = 10;
float baumgarteCorrectionPercent = 0.2f;
float baumgarteCorrectionSlop = 0.01f;
uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
int manifoldCacheLifetime = 6;
uint32_t frameCounter = 0;
pod::BVH::UpdatePolicy bvhUpdatePolicy = {
.displacementThreshold = 0.25f,
.overlapThreshold = 2.0f,
.dirtyRatioThreshold = 0.3f,
.maxFramesBeforeRebuild = 3600,
};
}
#define EPS(x) x // 1.0e-6f
@ -103,17 +122,38 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
if ( bodies.empty() ) return;
for ( auto* body : bodies ) {
if ( !body->activity.awake ) continue;
::integrate( *body, dt );
}
uf::stl::vector<pod::Manifold> manifolds;
uf::stl::vector<std::pair<int,int>> pairs;
// create BVH
::buildBroadphaseBVH( bvh, bodies, ::broadphaseBvhCapacity );
switch ( ::decideBVHUpdate( bvh, bodies, ::bvhUpdatePolicy, ::frameCounter++ ) ) {
case pod::BVH::UpdatePolicy::Decision::REBUILD: {
::buildBroadphaseBVH( bvh, bodies, ::broadphaseBvhCapacity ); // (re)build
} break;
case pod::BVH::UpdatePolicy::Decision::REFIT: {
::refitBVH( bvh, bodies ); // refit
} break;
case pod::BVH::UpdatePolicy::Decision::NONE:
default:
// no-op
break;
}
// query for overlaps
pod::BVH::pairs_t pairs;
::queryOverlaps( bvh, pairs );
::deduplicatePairs( pairs );
// build islands
uf::stl::vector<pod::Island> islands;
::buildIslands( pairs, world.bodies, islands );
// update sleep state per island
for ( auto& island : islands ) ::updateIsland( island, dt );
// iterate overlaps
uf::stl::vector<pod::Manifold> manifolds;
manifolds.reserve(::reserveCount);
for ( auto& [ia, ib] : pairs ) {
auto& a = *bodies[ia];
auto& b = *bodies[ib];
@ -138,7 +178,9 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
::reduceContacts( manifold );
// no points remained, skip
if ( manifold.points.empty() ) continue;
// wake up bodies
if ( a.activity.awake && !b.activity.awake ) ::wakeBody( b );
if ( b.activity.awake && !a.activity.awake ) ::wakeBody( a );
// store manifold
manifolds.emplace_back(manifold);
}
@ -146,24 +188,10 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
// pass manifolds to solver
::solveContacts( manifolds, dt );
// do position correction
::solvePositions( manifolds, dt );
if ( ::warmupSolver ) {
// update cache
for ( auto& manifold : manifolds ) {
::manifoldsCache[::makePairKey( *manifold.a, *manifold.b )] = manifold;
}
// prune if too old / empty
for ( auto& [ key, manifold ] : manifoldsCache ) {
// prune manifolds that are X frames old
for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) {
if ( it->lifetime > 3 ) manifold.points.erase(it);
else ++it;
}
// empty manifold, kill it
if ( manifold.points.empty() ) manifoldsCache.erase(key);
}
}
if ( ::warmupSolver ) ::storeManifolds( manifolds, ::manifoldsCache );
// recompute bounds for further queries
for ( auto* body : bodies ) {
@ -255,11 +283,11 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) {
}
}
void uf::physics::impl::applyForce( pod::PhysicsBody& body, const pod::Vector3f& force ) {
if ( body.isStatic ) return;
if ( body.isStatic ) return; ::wakeBody( body );
body.forceAccumulator += force;
}
void uf::physics::impl::applyForceAtPoint( pod::PhysicsBody body, const pod::Vector3f& force, const pod::Vector3f& point ) {
if ( body.isStatic ) return;
if ( body.isStatic ) return; ::wakeBody( body );
// linear force
body.forceAccumulator += force;
// angular force
@ -267,21 +295,23 @@ void uf::physics::impl::applyForceAtPoint( pod::PhysicsBody body, const pod::Vec
body.torqueAccumulator += uf::vector::cross( r, force );
}
void uf::physics::impl::applyImpulse( pod::PhysicsBody& body, const pod::Vector3f& impulse ) {
if ( body.isStatic ) return;
if ( body.isStatic ) return; ::wakeBody( body );
body.velocity += impulse * body.inverseMass;
}
void uf::physics::impl::applyTorque( pod::PhysicsBody& body, const pod::Vector3f& torque ) {
if ( body.isStatic ) return;
if ( body.isStatic ) return; ::wakeBody( body );
body.torqueAccumulator += torque;
}
void uf::physics::impl::setVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) {
::wakeBody( body );
body.velocity = v;
}
void uf::physics::impl::applyRotation( pod::PhysicsBody& body, const pod::Quaternion<>& q ) {
::wakeBody( body );
uf::transform::rotate( *body.transform/*.reference*/, q );
}
void uf::physics::impl::applyRotation( pod::PhysicsBody& body, const pod::Vector3f& axis, float angle ) {
applyRotation( body, uf::quaternion::axisAngle( axis, angle ) );
uf::physics::impl::applyRotation( body, uf::quaternion::axisAngle( axis, angle ) );
}
// body creation
@ -300,10 +330,13 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
if ( body.isStatic ) {
uf::physics::impl::setColliderCategory(body, "STATIC");
uf::physics::impl::setColliderMask(body, "STATIC");
} else {
uf::physics::impl::setColliderCategory(body, "DYNAMIC");
uf::physics::impl::setColliderMask(body, "DYNAMIC");
}
// insert into world
world.bodies.emplace_back(&body);
world.bodies.emplace_back(&body); // insert into world
world.bvh.dirty = true; // mark as dirty
return body;
}
@ -351,9 +384,10 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
auto& body = uf::physics::impl::create( world, object, mass, offset );
body.collider.type = pod::ShapeType::MESH;
body.collider.u.mesh.mesh = &mesh;
body.collider.u.mesh.bvh = new pod::BVH;
::buildMeshBVH( *body.collider.u.mesh.bvh, mesh, ::meshBvhCapacity );
body.collider.u.mesh.bvh = new pod::BVH;
auto& bvh = *body.collider.u.mesh.bvh;
::buildMeshBVH( bvh, mesh, ::meshBvhCapacity );
body.bounds = ::computeAABB( body );
uf::physics::impl::updateInertia( body );
@ -428,7 +462,7 @@ pod::RayQuery uf::physics::impl::rayCast( const pod::Ray& ray, const pod::World&
auto& bvh = world.bvh;
auto& bodies = world.bodies;
uf::stl::vector<int> candidates;
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, ray, candidates );
for ( auto i : candidates ) {

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

@ -114,33 +114,38 @@ namespace {
if ( manifold.points.size() <= 4 ) return;
uf::stl::vector<pod::Contact> result;
result.reserve(4);
for ( auto& c : manifold.points ) {
// prune invalid contacts
if ( !uf::vector::isValid( c.point ) ) continue;
if ( !uf::vector::isValid(c.point) ) continue;
bool merged = false;
for ( auto& r : result ) {
if ( !::similarContact( c, r ) ) continue;
// merge, pick deeper penetration
if ( !::similarContact(c, r) ) continue;
if ( c.penetration > r.penetration ) r = c;
merged = true;
break;
}
if ( !merged ) result.emplace_back(c);
if ( !merged ) {
if ( result.size() < 4 ) {
result.emplace_back(c);
} else {
// Replace weakest if this one is stronger
int weakest = 0;
for ( auto i = 1; i < 4; i++ ) {
if ( result[i].penetration < result[weakest].penetration ) weakest = i;
}
if ( c.penetration > result[weakest].penetration ) result[weakest] = c;
}
}
}
// UF_MSG_DEBUG("Reduced {} => {} contacts", manifold.points.size(), result.size());
// keep only deepest + farthest up to 4
std::sort(result.begin(), result.end(), [](auto& a, auto& b){ return a.penetration > b.penetration; });
if ( result.size() > 4 ) result.resize(4);
manifold.points = result;
}
void mergeContacts( pod::Manifold& manifold ) {
uf::stl::vector<pod::Contact> result;
result.reserve(4);
for ( auto& c : manifold.points ) {
bool merged = false;
for ( auto& r : result ) {
@ -172,6 +177,28 @@ namespace {
}
}
void storeManifolds( uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& manifoldsCache ){
// update cache
for ( auto& manifold : manifolds ) {
manifoldsCache[::makePairKey( *manifold.a, *manifold.b )] = manifold;
}
// prune if too old / empty
for ( auto itCache = manifoldsCache.begin(); itCache != manifoldsCache.end(); ) {
auto& manifold = itCache->second;
// prune manifolds that are X frames old
for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) {
if ( it->lifetime > ::manifoldCacheLifetime ) it = manifold.points.erase(it);
else ++it;
}
// empty manifold, kill it
if ( manifold.points.empty() ) itCache = manifoldsCache.erase(itCache);
else ++itCache;
}
}
void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) {
if ( !c.lifetime ) return; // too new
@ -198,12 +225,22 @@ namespace {
// baumgarte position correction
void positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact ) {
if ( ::baumgarteCorrectionPercent <= 0 ) return;
if ( a.isStatic && b.isStatic ) return;
float correctionMagnitude = std::max(contact.penetration - ::baumgarteCorrectionSlop, 0.0f) / (a.inverseMass + b.inverseMass) * ::baumgarteCorrectionPercent;
pod::Vector3f correction = contact.normal * correctionMagnitude;
// penetration depth beyond slop
float penetration = std::max( contact.penetration - ::baumgarteCorrectionSlop, 0.0f );
if ( penetration <= 0.0f ) return;
if ( !a.isStatic ) a.transform/*.reference*/->position -= correction * a.inverseMass;
if ( !b.isStatic ) b.transform/*.reference*/->position += correction * b.inverseMass;
// compute correction magnitude
float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
float totalInvMass = invMassA + invMassB;
if ( totalInvMass <= EPS(1e-8f) ) return;
// apply correction vector
pod::Vector3f correction = contact.normal * (penetration / totalInvMass) * ::baumgarteCorrectionPercent;
if ( !a.isStatic ) a.transform->position -= correction * invMassA;
if ( !b.isStatic ) b.transform->position += correction * invMassB;
}
void integrate( pod::PhysicsBody& body, float dt ) {
@ -236,6 +273,9 @@ namespace {
// apply rolling resistance
::applyRollingResistance(body, dt);
::applyRollingResistance( body, dt );
// update activity state
::updateActivity( body, dt );
}
}

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

@ -19,12 +19,12 @@ namespace {
const auto& mesh = a;
const auto& aabb = b;
const auto& meshData = *mesh.collider.u.mesh.mesh;
const auto& bvh = *mesh.collider.u.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh;
// transform to local space for BVH query
auto bounds = ::transformAabbToLocal( aabb.bounds, ::getTransform( mesh ) );
uf::stl::vector<int> candidates;
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, bounds, candidates );
bool hit = false;
@ -42,12 +42,12 @@ namespace {
const auto& mesh = a;
const auto& sphere = b;
const auto& meshData = *mesh.collider.u.mesh.mesh;
const auto& bvh = *mesh.collider.u.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh;
// transform to local space for BVH query
auto bounds = ::transformAabbToLocal( sphere.bounds, ::getTransform( mesh ) );
uf::stl::vector<int> candidates;
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, bounds, candidates );
bool hit = false;
@ -67,12 +67,12 @@ namespace {
const auto& mesh = a;
const auto& plane = b;
const auto& meshData = *mesh.collider.u.mesh.mesh;
const auto& bvh = *mesh.collider.u.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh;
// transform to local space for BVH query
auto bounds = ::transformAabbToLocal( plane.bounds, ::getTransform( mesh ) );
uf::stl::vector<int> candidates;
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, bounds, candidates );
bool hit = false;
@ -91,12 +91,12 @@ namespace {
const auto& mesh = a;
const auto& capsule = b;
const auto& meshData = *mesh.collider.u.mesh.mesh;
const auto& bvh = *mesh.collider.u.mesh.bvh;
const auto& meshData = *mesh.collider.u.mesh.mesh;
// transform to local space for BVH query
auto bounds = ::transformAabbToLocal( capsule.bounds, ::getTransform( mesh ) );
uf::stl::vector<int> candidates;
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, bounds, candidates );
bool hit = false;
@ -120,8 +120,9 @@ namespace {
const auto& bvhB = *b.collider.u.mesh.bvh;
// compute overlaps between one BVH and another BVH
pod::BVH::pair_t pairs;
::traverseNodePair(bvhA, 0, bvhB, 0, pairs);
pod::BVH::pairs_t pairs;
::queryOverlaps( bvhA, bvhB, pairs );
::deduplicatePairs( pairs );
bool hit = false;
// do collision per triangle

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

@ -193,8 +193,8 @@ namespace {
}
bool rayMesh( const pod::Ray& r, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) {
const uf::Mesh& meshData = *body.collider.u.mesh.mesh;
const pod::BVH& bvh = *body.collider.u.mesh.bvh;
const auto& meshData = *body.collider.u.mesh.mesh;
const auto& bvh = *body.collider.u.mesh.bvh;
const auto transform = ::getTransform( body );
@ -202,10 +202,10 @@ namespace {
ray.origin = uf::transform::applyInverse( transform, r.origin );
ray.direction = uf::quaternion::rotate( uf::quaternion::inverse( transform.orientation ), r.direction );
uf::stl::vector<int> indices;
::queryBVH( bvh, ray, indices );
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, ray, candidates );
for ( auto triID : indices ) {
for ( auto triID : candidates ) {
auto tri = ::fetchTriangle( meshData, triID );
float t, u, v;
@ -213,8 +213,7 @@ namespace {
if ( t >= rayHit.contact.penetration ) continue;
auto l = ray.origin + ray.direction * t;
auto bary = ::computeBarycentric( l, tri );
auto n = uf::vector::normalize( ::interpolateWithBarycentric( bary, tri.normals ) );
auto n = ::triangleNormal( tri );
// push back to world
auto p = uf::transform::apply( transform, l);

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

@ -70,7 +70,7 @@ namespace {
::applyImpulseTo(a, b, rA, rB, tangent * jt);
}
::positionCorrection(a, b, contact);
// ::positionCorrection(a, b, contact);
}
template<size_t N, typename T = float>
@ -143,7 +143,7 @@ namespace {
float cDot = vRel + penetrationBias;
rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; // RHS is magnitude of correction needed
rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; // rHS is magnitude of correction needed
lambda[i] = contact.accumulatedNormalImpulse;
}
@ -177,86 +177,99 @@ namespace {
return ::blockNxNSolver<4>( a, b, manifold, dt );
}
struct ContactCache {
pod::Vector3f normal;
pod::Vector3f tangent;
pod::Vector3f rA, rB;
float bias;
float effectiveMassN;
float effectiveMassT;
float accumulatedNormalImpulse;
float accumulatedTangentImpulse;
};
void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
const int count = std::min( (int) manifold.points.size(), 4 );
// precompute inv mass
float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
// precompute Jacobians for each contact
struct ContactCache {
pod::Vector3f normal;
pod::Vector3f rA, rB;
float bias;
float effectiveMass;
} cache[4];
const int count = std::min( (int)manifold.points.size(), 4 );
// precompute contact caches
::ContactCache cache[4];
for ( auto i = 0; i < count; i++ ) {
auto& c = manifold.points[i];
cache[i].normal = c.normal;
cache[i].rA = c.point - ::getPosition( a, true );
cache[i].rB = c.point - ::getPosition( b, true );
auto& cc = cache[i];
// bias = restitution + Baumgarte correction
float vRel = uf::vector::dot(
(b.velocity + uf::vector::cross(b.angularVelocity, cache[i].rB)) -
(a.velocity + uf::vector::cross(a.angularVelocity, cache[i].rA)),
cache[i].normal
);
cc.normal = c.normal;
cc.tangent = ::computeTangent( c.normal );
cc.rA = c.point - ::getPosition( a, true );
cc.rB = c.point - ::getPosition( b, true );
// relative velocity along normal
pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) );
float vn = uf::vector::dot( dv, cc.normal );
// restitution bias + baumgarte
float e = std::min( a.material.restitution, b.material.restitution );
float penetrationBias = std::max(c.penetration - ::baumgarteCorrectionSlop, 0.0f) * (::baumgarteCorrectionPercent / dt);
float penetrationBias = std::max( c.penetration - ::baumgarteCorrectionSlop, 0.0f ) * (::baumgarteCorrectionPercent / dt);
cc.bias = (vn < -1.0f ? -e * vn : 0.0f) + penetrationBias;
cache[i].bias = (vRel < -1.0f ? -e * vRel : 0.0f) + penetrationBias;
// effective mass (normal)
pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal );
pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal );
float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
uf::vector::dot( uf::vector::cross( rnA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rnB * b.inverseInertiaTensor, cc.rB ), cc.normal );
cc.effectiveMassN = (Kn > 0.0f) ? 1.0f / Kn : 0.0f;
// effective mass = 1 / (J M^-1 J^T)
pod::Vector3f rnA = uf::vector::cross( cache[i].rA, cache[i].normal );
pod::Vector3f rnB = uf::vector::cross( cache[i].rB, cache[i].normal );
// effective mass (tangent)
pod::Vector3f rtA = uf::vector::cross( cc.rA, cc.tangent );
pod::Vector3f rtB = uf::vector::cross( cc.rB, cc.tangent );
float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
uf::vector::dot( uf::vector::cross( rtA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rtB * b.inverseInertiaTensor, cc.rB ), cc.tangent );
cc.effectiveMassT = ( Kt > 0.0f ) ? ( 1.0f / Kt ) : 0.0f;
pod::Vector3f Ia_rnA = rnA * a.inverseInertiaTensor; // diag mult
pod::Vector3f Ib_rnB = rnB * b.inverseInertiaTensor;
// warm start
cc.accumulatedNormalImpulse = c.accumulatedNormalImpulse;
cc.accumulatedTangentImpulse = c.accumulatedTangentImpulse;
float Knormal = invMassA + invMassB + uf::vector::dot(uf::vector::cross(Ia_rnA, cache[i].rA) + uf::vector::cross(Ib_rnB, cache[i].rB), cache[i].normal);
cache[i].effectiveMass = (Knormal > 0.0f) ? 1.0f / Knormal : 0.0f;
// apply warm-start impulses
pod::Vector3f P = cc.normal * cc.accumulatedNormalImpulse + cc.tangent * cc.accumulatedTangentImpulse;
::applyImpulseTo(a, b, cc.rA, cc.rB, P);
}
// initialize lambdas (warm start)
pod::Vector4f lambda = {};
for ( auto i = 0; i < count; i++ ) {
lambda[i] = manifold.points[i].accumulatedNormalImpulse;
}
// iterative PGS loop
// iterative PGS
for ( auto iter = 0; iter < ::solverIterations; iter++ ) {
for (int i = 0; i < count; i++) {
auto& c = manifold.points[i];
for ( auto i = 0; i < count; i++ ) {
auto& cc = cache[i];
// relative velocity along normal
pod::Vector3f dv = (b.velocity + uf::vector::cross(b.angularVelocity, cc.rB)) - (a.velocity + uf::vector::cross(a.angularVelocity, cc.rA));
float vn = uf::vector::dot(dv, cc.normal);
// relative velocity
pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) );
// compute delta impulse
float delta = cc.effectiveMass * (-(vn + cc.bias));
// normal constraint
float vn = uf::vector::dot( dv, cc.normal );
float lambdaN = cc.effectiveMassN * (-(vn + cc.bias));
float oldImpulseN = cc.accumulatedNormalImpulse;
cc.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f );
float dPn = cc.accumulatedNormalImpulse - oldImpulseN;
// accumulate and clamp
float newLambda = std::max( lambda[i] + delta, 0.0f );
delta = newLambda - lambda[i];
lambda[i] = newLambda;
::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * dPn );
// apply impulse
::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * delta );
// friction constraint
dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) );
float vt = uf::vector::dot( dv, cc.tangent );
float lambdaT = cc.effectiveMassT * (-vt);
float maxFriction = ( a.material.dynamicFriction + b.material.dynamicFriction ) * 0.5f * cc.accumulatedNormalImpulse;
// position correction
::positionCorrection( a, b, c );
float oldImpulseT = cc.accumulatedTangentImpulse;
cc.accumulatedTangentImpulse = std::clamp( oldImpulseT + lambdaT, -maxFriction, maxFriction );
float dPt = cc.accumulatedTangentImpulse - oldImpulseT;
::applyImpulseTo( a, b, cc.rA, cc.rB, cc.tangent * dPt );
}
}
// store lambdas back
// store impulses back into manifold
for ( auto i = 0; i < count; i++ ) {
manifold.points[i].accumulatedNormalImpulse = lambda[i];
// ::positionCorrection( a, b, manifold.points[i] );
manifold.points[i].accumulatedNormalImpulse = cache[i].accumulatedNormalImpulse;
manifold.points[i].accumulatedTangentImpulse = cache[i].accumulatedTangentImpulse;
}
}
@ -274,4 +287,14 @@ namespace {
if ( ::warmupSolver ) for ( auto& manifold : manifolds ) ::warmupManifold( *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 ) {
for ( auto i = 0; i < iterations; ++i ) {
for ( auto& m : manifolds ) {
for ( auto& c : m.points ) {
::positionCorrection( *m.a, *m.b, c );
}
}
}
}
}

45
engine/src/utils/math/physics/tests.inl
View File

@ -928,11 +928,13 @@ TEST(TriangleTriangle_Collision_SimpleOverlap, {
pod::TriangleWithNormal triA{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::TriangleWithNormal triB{
{ { {0.2f,0.2f,0}, {0.8f,0.2f,0}, {0.2f,0.8f,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::impl::create( world, objA, triA, 0.0f );
@ -958,11 +960,13 @@ TEST(TriangleTriangle_Collision_CoplanarOverlap, {
pod::TriangleWithNormal triA{
{ { {0,0,0}, {2,0,0}, {0,2,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::TriangleWithNormal triB{
{ { {1,1,0}, {2,1,0}, {1,2,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::impl::create(world, objA, triA, 0.0f);
@ -984,11 +988,13 @@ TEST(TriangleTriangle_Collision_TouchingEdge, {
pod::TriangleWithNormal triA{
{ { {0,0,0}, {1,0,0}, {0.5f,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::TriangleWithNormal triB{
{ { {0.5f,1,0}, {1.5f,0,0}, {1,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::impl::create(world, objA, triA, 0.0f);
@ -1014,7 +1020,8 @@ TEST(TriangleAabb_Collision_OverlapCenter, {
pod::TriangleWithNormal tri {
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::AABB box = {{0.25f, 0.25f, -0.25f}, {0.75f, 0.75f, +0.25f}};
@ -1041,7 +1048,8 @@ TEST(TriangleAabb_Collision_NoOverlap, {
pod::TriangleWithNormal tri {
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::AABB box = {{2,2,2}, {3,3,3}};
@ -1066,7 +1074,8 @@ TEST(TrianglePlane_Collision_BelowPlane, {
pod::TriangleWithNormal tri{
{ { {0,0,-0.1f}, {1,0,-0.1f}, {0,1,-0.1f} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f);
@ -1093,7 +1102,8 @@ TEST(TrianglePlane_NoCollision_AbovePlane, {
pod::TriangleWithNormal tri{
{ { {0,0,1}, {1,0,1}, {0,1,1} } },
{ {0,0,-1}, {0,0,-1}, {0,0,-1} }, // facing down, but above plane
{0,0,-1},
//{ {0,0,-1}, {0,0,-1}, {0,0,-1} }, // facing down, but above plane
};
auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f);
@ -1113,7 +1123,8 @@ TEST(TriangleSphere_Collision_Tangent, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Sphere sphere = { 0.5f }; // radius
@ -1142,7 +1153,8 @@ TEST(TriangleCapsule_Collision_Overlap, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
// Capsule aligned along Z axis, radius 0.2
@ -1173,7 +1185,8 @@ TEST(TriangleCapsule_Collision_NoOverlap, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Capsule capsule;
@ -1198,7 +1211,8 @@ TEST(TriangleCapsule_Collision_Tangent, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Capsule capsule;
@ -1230,7 +1244,8 @@ TEST(TriangleCapsule_Collision_EdgeAlignment, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {2,0,0}, {0,2,0} } },
{ {0,0,1}, {0,0,1}, {0,0,1} },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Capsule capsule;

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

@ -25,7 +25,8 @@ namespace {
return uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0]));
}
pod::Vector3f triangleNormal( const pod::TriangleWithNormal& tri ) {
return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] );
return tri.normal;
//return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] );
}
pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) {
@ -57,16 +58,20 @@ namespace {
auto views = mesh.makeViews({"position", "normal"});
UF_ASSERT(!views.empty());
uint32_t triIndexID = triID * 3; // remap triangle ID to index ID
// find which view contains this triangle index.
size_t triBase = 0;
const uf::Mesh::View* found = nullptr;
for ( auto& v : views ) {
if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) {
auto trisInView = v.index.count / 3;
if (triID < triBase + trisInView) {
found = &v;
triID -= triBase; // local triangle index inside this view
break;
}
triBase += trisInView;
}
UF_ASSERT( found );
uint32_t triIndexID = triID * 3; // remap triangle ID to index ID
pod::TriangleWithNormal tri;
@ -77,9 +82,6 @@ namespace {
const void* indexBase = indices.data(found->index.first);
size_t indexSize = mesh.index.size;
// reset back to local indices range
triIndexID -= found->index.first;
uint32_t idxs[3];
// to-do: just make this a macro that could have a parallel hint
for ( auto i = 0; i < 3; ++i ) idxs[i] = getIndex(indexBase, indexSize, triIndexID + i);
@ -91,7 +93,10 @@ namespace {
for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
}
if ( normals.valid() ) {
tri.normal = uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0]));
/*
if ( false && normals.valid() ) {
auto* base = reinterpret_cast<const uint8_t*>(normals.data(found->vertex.first));
size_t stride = normals.stride();
for ( auto i = 0; i < 3; ++i ) tri.normals[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
@ -99,28 +104,29 @@ namespace {
auto normal = ::triangleNormal( (pod::Triangle&) tri );
for ( auto i = 0; i < 3; ++i ) tri.normals[i] = normal;
}
*/
return tri;
}
// if body is a mesh, apply its transform to the triangles, else reorient the normal with respect to the body
pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body ) {
pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body, bool fast = true ) {
auto tri = ::fetchTriangle( mesh, triID );
auto transform = ::getTransform( body );
if ( body.collider.type == pod::ShapeType::MESH ) {
for ( auto i = 0; i < 3; ++i ) {
tri.points[i] = uf::transform::apply( transform, tri.points[i] );
tri.normals[i] = uf::quaternion::rotate( transform.orientation, tri.normals[i] );
if ( fast ) {
for ( auto i = 0; i < 3; ++i ) tri.points[i] += transform.position;
} else {
for ( auto i = 0; i < 3; ++i ) tri.points[i] = uf::transform::apply( transform, tri.points[i] );
tri.normal = uf::quaternion::rotate( transform.orientation, tri.normal );
}
}
else {
#if REORIENT_NORMALS_ON_FETCH
auto triCenter = ::triangleCenter( tri );
auto delta = ::getPosition( body ) - triCenter;
for ( auto i = 0; i < 3; ++i ) {
if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i];
}
if ( uf::vector::dot(tri.normal, delta) < 0.0f ) tri.normal = -tri.normal;
#endif
}
@ -129,6 +135,7 @@ namespace {
bool computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1, float eps = EPS(1e-6f) ) {
uf::stl::vector<pod::Vector3f> intersections;
intersections.reserve(3);
auto checkAndPush = [&]( const pod::Vector3f& pt ) {
// avoid duplicates
@ -218,6 +225,7 @@ namespace {
// if ( !::triangleTriangleIntersect( a, b ) ) return false;
uf::stl::vector<pod::Vector3f> axes = { ::triangleNormal( a ), ::triangleNormal( b ) };
axes.reserve(2+3);
pod::Vector3f p0 = {}, p1 = {};
if ( !::computeTriangleTriangleSegment(a, b, p0, p1) ) {
@ -280,7 +288,7 @@ namespace {
float penetration = tolerance - dist;
#if REORIENT_NORMALS_ON_CONTACT
if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal;
#endif
manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
@ -306,7 +314,7 @@ namespace {
float penetration = r - dist;
#if REORIENT_NORMALS_ON_CONTACT
if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal;
#endif
manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
@ -379,7 +387,7 @@ namespace {
float penetration = r - dist;
#if REORIENT_NORMALS_ON_CONTACT
if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal;
#endif
manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });