more refinements and optimizations for the physics
This commit is contained in:
parent
fff258917e
commit
cb36936411
@ -141,7 +141,7 @@ namespace pod {
|
||||
pod::Capsule capsule;
|
||||
pod::TriangleWithNormal triangle;
|
||||
pod::MeshBVH mesh;
|
||||
} u;
|
||||
};
|
||||
};
|
||||
|
||||
struct PhysicsMaterial {
|
||||
@ -217,7 +217,8 @@ namespace pod {
|
||||
|
||||
struct Island {
|
||||
bool awake = true;
|
||||
uf::stl::vector<pod::PhysicsBody*> bodies;
|
||||
uf::stl::vector<uint32_t> indices;
|
||||
pod::BVH::pairs_t pairs;
|
||||
};
|
||||
|
||||
struct World {
|
||||
|
||||
@ -21,7 +21,7 @@ namespace {
|
||||
|
||||
std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& 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} );
|
||||
|
||||
// segment defines the cylinder axis only (ignore spherical ends)
|
||||
@ -35,25 +35,25 @@ namespace {
|
||||
switch ( body.collider.type ) {
|
||||
case pod::ShapeType::AABB: {
|
||||
return {
|
||||
transform.position + body.collider.u.aabb.min,
|
||||
transform.position + body.collider.u.aabb.max,
|
||||
transform.position + body.collider.aabb.min,
|
||||
transform.position + body.collider.aabb.max,
|
||||
};
|
||||
} break;
|
||||
case pod::ShapeType::SPHERE: {
|
||||
return {
|
||||
transform.position - body.collider.u.sphere.radius,
|
||||
transform.position + body.collider.u.sphere.radius,
|
||||
transform.position - body.collider.sphere.radius,
|
||||
transform.position + body.collider.sphere.radius,
|
||||
};
|
||||
} break;
|
||||
case pod::ShapeType::CAPSULE: {
|
||||
auto [ p1, p2 ] = ::getCapsuleSegment( body );
|
||||
return ::computeSegmentAABB( p1, p2, body.collider.u.capsule.radius );
|
||||
return ::computeSegmentAABB( p1, p2, body.collider.capsule.radius );
|
||||
} break;
|
||||
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 {
|
||||
transform.position + body.collider.u.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.min,
|
||||
transform.position + body.collider.mesh.bvh->nodes[0].bounds.max,
|
||||
};
|
||||
} break;
|
||||
default: {
|
||||
@ -193,7 +193,7 @@ namespace {
|
||||
auto overlaps = uf::vector::min( A.max, B.max ) - uf::vector::max( A.min, B.min );
|
||||
|
||||
// determine collision axis = smallest overlap
|
||||
int axis = -1;
|
||||
auto axis = -1;
|
||||
float minOverlap = FLT_MAX;
|
||||
for ( auto i = 0; i < 3; ++i ) {
|
||||
if ( overlaps[i] < minOverlap ) {
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
// BVH
|
||||
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{};
|
||||
node.left = -1;
|
||||
node.right = -1;
|
||||
@ -11,19 +11,19 @@ namespace {
|
||||
// compute bounds of this node
|
||||
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 ) {
|
||||
// leaf
|
||||
node.start = start;
|
||||
node.count = count;
|
||||
int index = (int) bvh.nodes.size();
|
||||
int32_t index = (int32_t) bvh.nodes.size();
|
||||
bvh.nodes.emplace_back(node);
|
||||
return index;
|
||||
}
|
||||
|
||||
// choose split axis by largest extent
|
||||
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
|
||||
std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](uint32_t a, uint32_t b) {
|
||||
@ -32,8 +32,8 @@ namespace {
|
||||
return ca < cb;
|
||||
});
|
||||
|
||||
int mid = ( start + end ) / 2;
|
||||
int index = (int) bvh.nodes.size();
|
||||
int32_t mid = ( start + end ) / 2;
|
||||
int32_t index = (int32_t) bvh.nodes.size();
|
||||
bvh.nodes.emplace_back( node ); // insert now, gets filled later
|
||||
|
||||
node.left = ::buildBVHNode( bvh, bounds, start, mid, capacity );
|
||||
@ -42,10 +42,10 @@ namespace {
|
||||
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 {
|
||||
pod::AABB bounds;
|
||||
int count = 0;
|
||||
int32_t count = 0;
|
||||
};
|
||||
|
||||
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]] );
|
||||
|
||||
int count = end - start;
|
||||
int32_t count = end - start;
|
||||
if ( count <= capacity ) {
|
||||
node.count = count;
|
||||
int index = (int) bvh.nodes.size();
|
||||
int32_t index = (int32_t) bvh.nodes.size();
|
||||
bvh.nodes.emplace_back(node);
|
||||
return index;
|
||||
}
|
||||
|
||||
constexpr int numBins = 16;
|
||||
constexpr auto 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;
|
||||
auto bestAxis = -1, bestSplit = -1;
|
||||
float bestCost = std::numeric_limits<float>::infinity();
|
||||
|
||||
for ( auto axis = 0; axis < 3; ++axis ) {
|
||||
@ -81,18 +81,18 @@ namespace {
|
||||
float scale = (float) numBins / (maxC - minC);
|
||||
|
||||
for ( auto i = start; i < end; ++i ) {
|
||||
int idx = bvh.indices[i];
|
||||
int32_t idx = bvh.indices[i];
|
||||
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].bounds = ::mergeAabb( bins[binID].bounds, bounds[idx] );
|
||||
}
|
||||
|
||||
pod::AABB leftBounds[numBins], rightBounds[numBins];
|
||||
int leftCount[numBins] = {}, rightCount[numBins] = {};
|
||||
int32_t leftCount[numBins] = {}, rightCount[numBins] = {};
|
||||
|
||||
pod::AABB acc;
|
||||
int cnt = 0;
|
||||
int32_t 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;
|
||||
@ -127,7 +127,7 @@ namespace {
|
||||
// fallback: no valid split → make leaf
|
||||
if ( bestAxis == -1 ) {
|
||||
node.count = count;
|
||||
int index = (int) bvh.nodes.size();
|
||||
int32_t index = (int32_t) bvh.nodes.size();
|
||||
bvh.nodes.emplace_back(node);
|
||||
return index;
|
||||
}
|
||||
@ -136,23 +136,23 @@ namespace {
|
||||
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) {
|
||||
auto midIt = std::partition( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](int32_t idx) {
|
||||
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;
|
||||
});
|
||||
|
||||
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 ( mid == start || mid == end ) {
|
||||
node.count = count;
|
||||
int index = (int) bvh.nodes.size();
|
||||
int32_t index = (int32_t) bvh.nodes.size();
|
||||
bvh.nodes.emplace_back(node);
|
||||
return index;
|
||||
}
|
||||
|
||||
int index = (int) bvh.nodes.size();
|
||||
int32_t index = (int32_t) bvh.nodes.size();
|
||||
bvh.nodes.emplace_back(node);
|
||||
|
||||
node.left = ::buildBVHNode_SAH( bvh, bounds, start, mid, capacity );
|
||||
@ -161,7 +161,7 @@ namespace {
|
||||
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;
|
||||
|
||||
bvh.indices.clear();
|
||||
@ -175,7 +175,7 @@ namespace {
|
||||
for ( auto i = 0; i < bodies.size(); ++i ) {
|
||||
if ( filters && bodies[i]->isStatic != filterType ) continue;
|
||||
|
||||
bounds[i] = ::computeAABB(*bodies[i]);
|
||||
bounds[i] = bodies[i]->bounds;
|
||||
bvh.indices.emplace_back(i);
|
||||
}
|
||||
|
||||
@ -191,8 +191,8 @@ namespace {
|
||||
bvh.dirty = false;
|
||||
}
|
||||
|
||||
void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int capacity = 4 ) {
|
||||
int triangles = mesh.index.count / 3;
|
||||
void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int32_t capacity = 4 ) {
|
||||
uint32_t triangles = mesh.index.count / 3;
|
||||
|
||||
bvh.indices.clear();
|
||||
bvh.nodes.clear();
|
||||
@ -235,22 +235,23 @@ 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 ) {
|
||||
if ( bvh.indices.empty() || bvh.nodes.empty() || bvh.dirty ) {
|
||||
UF_MSG_DEBUG("Force rebuild, bvh.indices.empty={}, bvh.nodes.empty={}, bvh.dirty={}", bvh.indices.empty(), bvh.nodes.empty(), bvh.dirty );
|
||||
pod::BVH::UpdatePolicy::Decision decideBVHUpdate( const pod::BVH& bvh, uf::stl::vector<pod::PhysicsBody*>& bodies, const pod::BVH::UpdatePolicy& policy, size_t frameCounter ) {
|
||||
// BVH is not built
|
||||
if ( bvh.indices.empty() || bvh.nodes.empty() ) {
|
||||
return pod::BVH::UpdatePolicy::Decision::REBUILD;
|
||||
}
|
||||
if ( bodies.empty() ) return pod::BVH::UpdatePolicy::Decision::NONE;
|
||||
|
||||
int dirtyCount = 0;
|
||||
uint32_t dirtyCount = 0;
|
||||
float oldRootArea = ::aabbSurfaceArea( bvh.nodes[0].bounds );
|
||||
|
||||
// check each body
|
||||
// update/check each body
|
||||
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
|
||||
pod::Vector3f oldCenter = ( oldBounds.min + oldBounds.max ) * 0.5f;
|
||||
@ -272,10 +273,11 @@ namespace {
|
||||
}
|
||||
|
||||
float newRootArea = ::aabbSurfaceArea( newRoot );
|
||||
if ( dirtyRatio > policy.dirtyRatioThreshold || newRootArea > oldRootArea * policy.overlapThreshold || frameCounter % policy.maxFramesBeforeRebuild == 0 ) {
|
||||
UF_MSG_DEBUG( "Rebuild, dirtyRatio={}, oldRootArea={}, newRootArea={}, frameCounter={}", dirtyRatio, oldRootArea, newRootArea, frameCounter );
|
||||
// BVH is too out of date, rebuild it
|
||||
if ( bvh.dirty || dirtyRatio > policy.dirtyRatioThreshold || newRootArea > oldRootArea * policy.overlapThreshold || frameCounter % policy.maxFramesBeforeRebuild == 0 ) {
|
||||
return pod::BVH::UpdatePolicy::Decision::REBUILD;
|
||||
}
|
||||
// bodies moved, refit the BVH instead
|
||||
if ( dirtyCount > 0 ) return pod::BVH::UpdatePolicy::Decision::REFIT;
|
||||
return pod::BVH::UpdatePolicy::Decision::NONE;
|
||||
}
|
||||
@ -293,14 +295,14 @@ namespace {
|
||||
// leaf node: recompute bounds from bodies
|
||||
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]] );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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];
|
||||
// internal node
|
||||
if ( node.count == 0 ) {
|
||||
@ -321,19 +323,19 @@ namespace {
|
||||
// leaf node: recompute bounds from bodies
|
||||
auto nodeID = bvh.indices[node.start];
|
||||
|
||||
node.bounds = ::computeAABB( *bodies[nodeID] );
|
||||
node.bounds = bodies[nodeID]->bounds;
|
||||
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];
|
||||
node.bounds = ::mergeAabb(node.bounds, ::computeAABB( *bodies[bodyID] ) );
|
||||
node.bounds = ::mergeAabb(node.bounds, bodies[bodyID]->bounds );
|
||||
node.asleep = node.asleep && !bodies[bodyID]->activity.awake;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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];
|
||||
// internal node
|
||||
if ( node.count == 0 ) {
|
||||
@ -346,7 +348,7 @@ namespace {
|
||||
}
|
||||
|
||||
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;
|
||||
bounds.reserve( triangles );
|
||||
@ -373,12 +375,12 @@ 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());
|
||||
|
||||
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
|
||||
|
||||
pod::BVH::FlatNode flat{};
|
||||
@ -401,8 +403,8 @@ namespace {
|
||||
flat.start = -1;
|
||||
flat.count = 0;
|
||||
|
||||
int leftID = ::flattenBVH( bvh, node.left );
|
||||
int rightID = ::flattenBVH( bvh, node.right );
|
||||
int32_t leftID = ::flattenBVH( bvh, node.left );
|
||||
int32_t rightID = ::flattenBVH( bvh, node.right );
|
||||
|
||||
flat.skipIndex = rightID; // skip entire subtree
|
||||
bvh.flattened[flatID] = flat;
|
||||
@ -413,7 +415,7 @@ namespace {
|
||||
|
||||
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) {
|
||||
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& nodeB = bvh.nodes[nodeBID];
|
||||
|
||||
@ -422,8 +424,8 @@ namespace {
|
||||
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];
|
||||
int32_t bodyA = bvh.indices[nodeA.start + i];
|
||||
int32_t bodyB = bvh.indices[nodeB.start + j];
|
||||
if ( bodyA == bodyB ) continue;
|
||||
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)
|
||||
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& nodeB = bvhB.nodes[nodeBID];
|
||||
|
||||
@ -452,8 +454,8 @@ namespace {
|
||||
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];
|
||||
int32_t bodyA = bvhA.indices[nodeA.start + i];
|
||||
int32_t bodyB = bvhB.indices[nodeB.start + j];
|
||||
if ( bodyA == bodyB ) continue;
|
||||
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];
|
||||
|
||||
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];
|
||||
int32_t bodyA = bvh.indices[node.start + i];
|
||||
int32_t bodyB = bvh.indices[node.start + j];
|
||||
|
||||
if ( bodyA == bodyB ) continue;
|
||||
if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
|
||||
@ -524,7 +526,7 @@ namespace {
|
||||
stack.push(0);
|
||||
|
||||
while ( !stack.empty() ) {
|
||||
int idx = stack.top(); stack.pop();
|
||||
int32_t idx = stack.top(); stack.pop();
|
||||
auto& node = bvh.nodes[idx];
|
||||
if ( node.asleep || !::aabbOverlap( bounds, node.bounds ) ) continue;
|
||||
|
||||
@ -541,7 +543,7 @@ namespace {
|
||||
}
|
||||
|
||||
// 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 ( nodeID == 0 ) outIndices.reserve(::reserveCount);
|
||||
@ -570,7 +572,7 @@ namespace {
|
||||
stack.push(0);
|
||||
|
||||
while ( !stack.empty() ) {
|
||||
int idx = stack.top(); stack.pop();
|
||||
int32_t idx = stack.top(); stack.pop();
|
||||
const auto& node = bvh.nodes[idx];
|
||||
|
||||
float tMin, tMax;
|
||||
@ -586,7 +588,7 @@ namespace {
|
||||
}
|
||||
}
|
||||
// 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 ( nodeID == 0 ) outIndices.reserve(::reserveCount);
|
||||
@ -615,11 +617,11 @@ namespace {
|
||||
|
||||
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];
|
||||
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];
|
||||
if ( nodeB.count <= 0 || nodeB.asleep ) continue;
|
||||
|
||||
@ -650,11 +652,11 @@ namespace {
|
||||
|
||||
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];
|
||||
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];
|
||||
if ( nodeB.count <= 0 || nodeB.asleep ) continue;
|
||||
|
||||
@ -678,14 +680,14 @@ namespace {
|
||||
|
||||
outIndices.reserve(::reserveCount);
|
||||
|
||||
int idx = 0;
|
||||
while ( idx < (int) nodes.size() ) {
|
||||
int32_t idx = 0;
|
||||
while ( idx < 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 ) {
|
||||
for ( auto i = 0; i < node.count; ++i ) {
|
||||
outIndices.emplace_back( indices[node.start + i] );
|
||||
}
|
||||
}
|
||||
@ -702,14 +704,14 @@ namespace {
|
||||
|
||||
outIndices.reserve(::reserveCount);
|
||||
|
||||
int idx = 0;
|
||||
while ( idx < (int) nodes.size() ) {
|
||||
int32_t idx = 0;
|
||||
while ( idx < 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 ) {
|
||||
for ( auto i = 0; i < node.count; ++i ) {
|
||||
outIndices.emplace_back( indices[node.start + i] );
|
||||
}
|
||||
}
|
||||
@ -723,47 +725,80 @@ 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 ) {
|
||||
islands.reserve(::reserveCount);
|
||||
UnionFind unionizer(bodies.size());
|
||||
|
||||
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
|
||||
// union all pairs
|
||||
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);
|
||||
unionizer.unite(a, b);
|
||||
}
|
||||
|
||||
// map root to island index
|
||||
uf::stl::unordered_map<int32_t, int32_t> rootToIsland;
|
||||
|
||||
islands.clear();
|
||||
islands.reserve(bodies.size());
|
||||
|
||||
for ( auto i = 0; i < bodies.size(); i++ ) {
|
||||
int32_t root = unionizer.find(i);
|
||||
|
||||
if (rootToIsland.find(root) == rootToIsland.end()) {
|
||||
rootToIsland[root] = (int32_t) islands.size();
|
||||
islands.emplace_back();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
islands.emplace_back( std::move( island ) );
|
||||
}
|
||||
}
|
||||
bool updateIsland( pod::Island& island, uf::stl::vector<pod::PhysicsBody*>& bodies, float dt ) {
|
||||
island.awake = false;
|
||||
|
||||
void updateIsland( pod::Island& island, float dt ) {
|
||||
bool allStill = true;
|
||||
|
||||
for ( auto* b : island.bodies ) {
|
||||
auto& body = *b;
|
||||
for ( auto idx : island.indices ) {
|
||||
auto& body = *bodies[idx];
|
||||
if ( !body.activity.awake ) continue;
|
||||
|
||||
float linSpeed = uf::vector::norm( body.velocity );
|
||||
@ -773,23 +808,18 @@ namespace {
|
||||
body.activity.sleepTimer += dt;
|
||||
} else {
|
||||
body.activity.sleepTimer = 0.0f;
|
||||
allStill = false;
|
||||
island.awake = true;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// update bodies within island
|
||||
for ( auto idx : island.indices )
|
||||
(island.awake ? ::wakeBody : ::sleepBody)( *bodies[idx] );
|
||||
|
||||
return island.awake;
|
||||
}
|
||||
}
|
||||
@ -6,7 +6,7 @@ namespace {
|
||||
auto [ B1, B2 ] = ::getCapsuleSegment( b );
|
||||
|
||||
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;
|
||||
float dist2 = uf::vector::dot(delta, delta);
|
||||
@ -26,7 +26,7 @@ namespace {
|
||||
const auto& box = b;
|
||||
|
||||
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 closestSegment = ::closestPointOnSegment( closestPoint, p1, p2 );
|
||||
@ -48,11 +48,11 @@ namespace {
|
||||
const auto& capsule = a;
|
||||
const auto& plane = b;
|
||||
|
||||
const auto& normal = plane.collider.u.plane.normal;
|
||||
float o = plane.collider.u.plane.offset;
|
||||
const auto& normal = plane.collider.plane.normal;
|
||||
float o = plane.collider.plane.offset;
|
||||
|
||||
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
|
||||
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 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
|
||||
auto closest = ::closestPointOnSegment( sphereCenter, p1, p2 );
|
||||
@ -97,7 +97,7 @@ namespace {
|
||||
|
||||
float penetration = r - dist;
|
||||
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 });
|
||||
return true;
|
||||
|
||||
@ -51,7 +51,7 @@ namespace {
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
remove.reserve( faces.size() );
|
||||
@ -68,8 +68,8 @@ namespace {
|
||||
}
|
||||
|
||||
// remove visible faces
|
||||
for ( auto i = (int) remove.size() - 1; i >= 0; --i ) {
|
||||
int idx = remove[i];
|
||||
for ( auto i = (int32_t) remove.size() - 1; i >= 0; --i ) {
|
||||
auto idx = remove[i];
|
||||
faces[idx] = faces.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) );
|
||||
|
||||
auto faces = ::initialPolytope(simplex);
|
||||
@ -88,9 +88,9 @@ namespace {
|
||||
|
||||
for ( auto it = 0; it < maxIterations; ++it ) {
|
||||
// find closest face
|
||||
int idx = -1;
|
||||
int32_t idx = -1;
|
||||
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 ) {
|
||||
minDist = faces[i].distance;
|
||||
idx = i;
|
||||
|
||||
@ -3,7 +3,7 @@ namespace {
|
||||
const auto transform = ::getTransform( body );
|
||||
switch ( body.collider.type ) {
|
||||
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;
|
||||
case pod::ShapeType::AABB: {
|
||||
return {
|
||||
@ -14,13 +14,13 @@ namespace {
|
||||
} break;
|
||||
case pod::ShapeType::CAPSULE: {
|
||||
auto up = uf::quaternion::rotate( transform.orientation, pod::Vector3f{0,1,0} );
|
||||
auto p1 = transform.position + up * body.collider.u.capsule.halfHeight;
|
||||
auto p2 = transform.position - up * body.collider.u.capsule.halfHeight;
|
||||
auto p1 = transform.position + up * body.collider.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
|
||||
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: {
|
||||
const auto& tri = body.collider.u.triangle;
|
||||
const auto& tri = body.collider.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 );
|
||||
|
||||
@ -43,7 +43,7 @@ namespace {
|
||||
|
||||
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& a, int32_t nodeA, const pod::BVH& b, int32_t nodeB, pod::BVH::pairs_t& out );
|
||||
|
||||
@ -11,12 +11,12 @@ namespace {
|
||||
bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK
|
||||
bool fixedStep = true;
|
||||
|
||||
int substeps = 0;
|
||||
int reserveCount = 32;
|
||||
int32_t substeps = 0;
|
||||
int32_t reserveCount = 32;
|
||||
|
||||
// increasing these make things lag
|
||||
int broadphaseBvhCapacity = 1;
|
||||
int meshBvhCapacity = 1;
|
||||
int32_t broadphaseBvhCapacity = 1;
|
||||
int32_t meshBvhCapacity = 1;
|
||||
|
||||
bool flattenBvhBodies = false; // bugged
|
||||
bool flattenBvhMeshes = true;
|
||||
@ -27,12 +27,12 @@ namespace {
|
||||
|
||||
bool useSplitBvhs = true; // currently bugged if enabled
|
||||
|
||||
int solverIterations = 10;
|
||||
int32_t solverIterations = 10;
|
||||
float baumgarteCorrectionPercent = 0.2f;
|
||||
float baumgarteCorrectionSlop = 0.01f;
|
||||
|
||||
uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
|
||||
int manifoldCacheLifetime = 6;
|
||||
int32_t manifoldCacheLifetime = 6;
|
||||
|
||||
uint32_t frameCounter = 0;
|
||||
pod::BVH::UpdatePolicy bvhUpdatePolicy = {
|
||||
@ -111,7 +111,7 @@ void uf::physics::impl::terminate( pod::World& world ) {
|
||||
}
|
||||
|
||||
// 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;
|
||||
for ( auto i=0; i < substeps; ++i) {
|
||||
uf::physics::impl::step( world, h );
|
||||
@ -131,7 +131,7 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
|
||||
::integrate( *body, dt );
|
||||
}
|
||||
|
||||
// rebuild static bvh if diry
|
||||
// rebuild static bvh if dirty
|
||||
if ( staticBvh.dirty && ::useSplitBvhs ) {
|
||||
::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
|
||||
} break;
|
||||
case pod::BVH::UpdatePolicy::Decision::NONE:
|
||||
default:
|
||||
default: {
|
||||
// no-op
|
||||
break;
|
||||
} break;
|
||||
}
|
||||
|
||||
// query for overlaps
|
||||
@ -156,26 +156,27 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
|
||||
::queryOverlaps( dynamicBvh, staticBvh, pairs );
|
||||
}
|
||||
|
||||
// build islands
|
||||
// build islands from overlaps
|
||||
uf::stl::vector<pod::Island> islands;
|
||||
::buildIslands( pairs, bodies, islands );
|
||||
|
||||
// update sleep state per island
|
||||
for ( auto& island : islands ) ::updateIsland( island, dt );
|
||||
|
||||
// iterate overlaps
|
||||
// iterate islands
|
||||
#pragma omp parallel for schedule(dynamic)
|
||||
for ( auto& island : islands ) {
|
||||
uf::stl::vector<pod::Manifold> manifolds;
|
||||
manifolds.reserve(::reserveCount);
|
||||
|
||||
for ( auto [ia, ib] : pairs ) {
|
||||
// sleeping island, skip
|
||||
if ( !::updateIsland( island, bodies, dt ) ) continue;
|
||||
// iterate overlap pairs
|
||||
for ( auto& [ ia, ib ] : island.pairs ) {
|
||||
auto& a = *bodies[ia];
|
||||
auto& b = *bodies[ib];
|
||||
|
||||
// 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 ) ) {
|
||||
// did not collide
|
||||
if ( !::generateContacts( a, b, manifold, dt ) ) continue;
|
||||
|
||||
// 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
|
||||
if ( a.collider.type != pod::ShapeType::MESH && b.collider.type != pod::ShapeType::MESH ) {
|
||||
@ -198,19 +199,13 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
|
||||
// store manifold
|
||||
manifolds.emplace_back(manifold);
|
||||
}
|
||||
}
|
||||
|
||||
// pass manifolds to solver
|
||||
::solveContacts( manifolds, dt );
|
||||
// do position correction
|
||||
::solvePositions( manifolds, dt );
|
||||
|
||||
// cache manifold positions
|
||||
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 ) {
|
||||
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;
|
||||
|
||||
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 };
|
||||
} break;
|
||||
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;
|
||||
body.inertiaTensor = { I, I, I };
|
||||
body.inverseInertiaTensor = { invI, invI, invI };
|
||||
} break;
|
||||
case pod::ShapeType::CAPSULE: {
|
||||
float r = body.collider.u.capsule.radius;
|
||||
float h = body.collider.u.capsule.halfHeight * 2.0f; // full cyl height
|
||||
float r = body.collider.capsule.radius;
|
||||
float h = body.collider.capsule.halfHeight * 2.0f; // full cyl height
|
||||
float m = body.mass;
|
||||
|
||||
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 ) {
|
||||
auto& body = uf::physics::impl::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::AABB;
|
||||
body.collider.u.aabb = aabb;
|
||||
body.collider.aabb = aabb;
|
||||
body.bounds = ::computeAABB( body );
|
||||
uf::physics::impl::updateInertia( 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 ) {
|
||||
auto& body = uf::physics::impl::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::SPHERE;
|
||||
body.collider.u.sphere = sphere;
|
||||
body.collider.sphere = sphere;
|
||||
body.bounds = ::computeAABB( body );
|
||||
uf::physics::impl::updateInertia( 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 ) {
|
||||
auto& body = uf::physics::impl::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::PLANE;
|
||||
body.collider.u.plane = plane;
|
||||
body.collider.plane = plane;
|
||||
body.bounds = ::computeAABB( body );
|
||||
uf::physics::impl::updateInertia( 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 ) {
|
||||
auto& body = uf::physics::impl::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::CAPSULE;
|
||||
body.collider.u.capsule = capsule;
|
||||
body.collider.capsule = capsule;
|
||||
body.bounds = ::computeAABB( body );
|
||||
uf::physics::impl::updateInertia( 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 ) {
|
||||
auto& body = uf::physics::impl::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::TRIANGLE;
|
||||
body.collider.u.triangle = tri;
|
||||
body.collider.triangle = tri;
|
||||
body.bounds = ::computeAABB( body );
|
||||
uf::physics::impl::updateInertia( 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 ) {
|
||||
auto& body = uf::physics::impl::create( world, object, mass, offset );
|
||||
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;
|
||||
auto& bvh = *body.collider.u.mesh.bvh;
|
||||
body.collider.mesh.bvh = new pod::BVH;
|
||||
auto& bvh = *body.collider.mesh.bvh;
|
||||
::buildMeshBVH( bvh, mesh, ::meshBvhCapacity );
|
||||
|
||||
body.bounds = ::computeAABB( body );
|
||||
@ -461,7 +456,7 @@ void uf::physics::impl::destroy( pod::PhysicsBody& body ) {
|
||||
|
||||
// remove any pointered collider data
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -131,7 +131,7 @@ namespace {
|
||||
result.emplace_back(c);
|
||||
} else {
|
||||
// Replace weakest if this one is stronger
|
||||
int weakest = 0;
|
||||
auto weakest = 0;
|
||||
for ( auto i = 1; i < 4; i++ ) {
|
||||
if ( result[i].penetration < result[weakest].penetration ) weakest = i;
|
||||
}
|
||||
@ -264,14 +264,10 @@ namespace {
|
||||
uf::transform::rotate( *body.transform/*.reference*/, dq );
|
||||
}
|
||||
|
||||
// update AABB
|
||||
body.bounds = ::computeAABB( body );
|
||||
|
||||
// reset accumulators
|
||||
body.forceAccumulator = {};
|
||||
body.torqueAccumulator = {};
|
||||
|
||||
|
||||
// apply rolling resistance
|
||||
::applyRollingResistance( body, dt );
|
||||
|
||||
|
||||
@ -19,8 +19,8 @@ namespace {
|
||||
const auto& mesh = a;
|
||||
const auto& aabb = b;
|
||||
|
||||
const auto& bvh = *mesh.collider.u.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.u.mesh.mesh;
|
||||
const auto& bvh = *mesh.collider.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.mesh.mesh;
|
||||
|
||||
// transform to local space for BVH query
|
||||
auto bounds = ::transformAabbToLocal( aabb.bounds, ::getTransform( mesh ) );
|
||||
@ -42,8 +42,8 @@ namespace {
|
||||
const auto& mesh = a;
|
||||
const auto& sphere = b;
|
||||
|
||||
const auto& bvh = *mesh.collider.u.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.u.mesh.mesh;
|
||||
const auto& bvh = *mesh.collider.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.mesh.mesh;
|
||||
|
||||
// transform to local space for BVH query
|
||||
auto bounds = ::transformAabbToLocal( sphere.bounds, ::getTransform( mesh ) );
|
||||
@ -67,8 +67,8 @@ namespace {
|
||||
const auto& mesh = a;
|
||||
const auto& plane = b;
|
||||
|
||||
const auto& bvh = *mesh.collider.u.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.u.mesh.mesh;
|
||||
const auto& bvh = *mesh.collider.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.mesh.mesh;
|
||||
|
||||
// transform to local space for BVH query
|
||||
auto bounds = ::transformAabbToLocal( plane.bounds, ::getTransform( mesh ) );
|
||||
@ -91,8 +91,8 @@ namespace {
|
||||
const auto& mesh = a;
|
||||
const auto& capsule = b;
|
||||
|
||||
const auto& bvh = *mesh.collider.u.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.u.mesh.mesh;
|
||||
const auto& bvh = *mesh.collider.mesh.bvh;
|
||||
const auto& meshData = *mesh.collider.mesh.mesh;
|
||||
|
||||
// transform to local space for BVH query
|
||||
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 ) {
|
||||
ASSERT_COLLIDER_TYPES( MESH, MESH );
|
||||
|
||||
const auto& bvhA = *a.collider.u.mesh.bvh;
|
||||
const auto& meshA = *a.collider.u.mesh.mesh;
|
||||
const auto& bvhA = *a.collider.mesh.bvh;
|
||||
const auto& meshA = *a.collider.mesh.mesh;
|
||||
|
||||
const auto& meshB = *b.collider.u.mesh.mesh;
|
||||
const auto& bvhB = *b.collider.u.mesh.bvh;
|
||||
const auto& meshB = *b.collider.mesh.mesh;
|
||||
const auto& bvhB = *b.collider.mesh.bvh;
|
||||
|
||||
// compute overlaps between one BVH and another BVH
|
||||
pod::BVH::pairs_t pairs;
|
||||
|
||||
@ -5,8 +5,8 @@ namespace {
|
||||
const auto& plane = a;
|
||||
const auto& aabb = b;
|
||||
|
||||
auto normal = uf::vector::normalize( plane.collider.u.plane.normal );
|
||||
float offset = plane.collider.u.plane.offset;
|
||||
auto normal = uf::vector::normalize( plane.collider.plane.normal );
|
||||
float offset = plane.collider.plane.offset;
|
||||
|
||||
auto center = ::aabbCenter( aabb.bounds ); // center
|
||||
auto extent = ::aabbExtent( aabb.bounds ); // half extents
|
||||
@ -27,11 +27,11 @@ namespace {
|
||||
const auto& plane = a;
|
||||
const auto& sphere = b;
|
||||
|
||||
auto& normal = plane.collider.u.plane.normal;
|
||||
float offset = plane.collider.u.plane.offset;
|
||||
auto& normal = plane.collider.plane.normal;
|
||||
float offset = plane.collider.plane.offset;
|
||||
|
||||
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;
|
||||
if ( dist > r ) return false;
|
||||
|
||||
@ -76,7 +76,7 @@ namespace {
|
||||
}
|
||||
bool raySphere( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) {
|
||||
auto center = ::getPosition(body);
|
||||
float r = body.collider.u.sphere.radius;
|
||||
float r = body.collider.sphere.radius;
|
||||
|
||||
// vector from sphere center to ray origin
|
||||
auto oc = ray.origin - center;
|
||||
@ -109,8 +109,8 @@ namespace {
|
||||
return true;
|
||||
}
|
||||
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;
|
||||
float offset = body.collider.u.plane.offset;
|
||||
auto& normal = body.collider.plane.normal;
|
||||
float offset = body.collider.plane.offset;
|
||||
|
||||
float denom = uf::vector::dot( normal, ray.direction );
|
||||
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 ) {
|
||||
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 m = ray.origin - p1;
|
||||
@ -193,8 +193,8 @@ namespace {
|
||||
}
|
||||
|
||||
bool rayMesh( const pod::Ray& r, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) {
|
||||
const auto& meshData = *body.collider.u.mesh.mesh;
|
||||
const auto& bvh = *body.collider.u.mesh.bvh;
|
||||
const auto& meshData = *body.collider.mesh.mesh;
|
||||
const auto& bvh = *body.collider.mesh.bvh;
|
||||
|
||||
const auto transform = ::getTransform( body );
|
||||
|
||||
|
||||
@ -87,11 +87,11 @@ namespace {
|
||||
auto pA = ::getPosition( a, 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 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 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 ) {
|
||||
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
|
||||
::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 );
|
||||
}
|
||||
|
||||
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& m : manifolds ) {
|
||||
pod::Contact s = {};
|
||||
float weight = 0;
|
||||
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 );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -3,7 +3,7 @@ namespace {
|
||||
ASSERT_COLLIDER_TYPES( SPHERE, SPHERE );
|
||||
|
||||
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);
|
||||
if ( dist2 > r * r ) return false;
|
||||
@ -11,7 +11,7 @@ namespace {
|
||||
|
||||
float penetration = r - dist;
|
||||
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 });
|
||||
return true;
|
||||
@ -23,7 +23,7 @@ namespace {
|
||||
const auto& aabb = b;
|
||||
|
||||
auto center = ::getPosition( sphere );
|
||||
float r = sphere.collider.u.sphere.radius;
|
||||
float r = sphere.collider.sphere.radius;
|
||||
|
||||
auto& bounds = aabb.bounds;
|
||||
auto closest = pod::Vector3f{
|
||||
|
||||
@ -133,7 +133,7 @@ namespace {
|
||||
// clip edges of A against plane of B
|
||||
const pod::Vector3f At[3] = { A.points[0], A.points[1], A.points[2] };
|
||||
for ( auto i = 0; i < 3; ++i ) {
|
||||
int j = ( i + 1 ) % 3;
|
||||
auto j = ( i + 1 ) % 3;
|
||||
pod::Vector3f p;
|
||||
if ( intersectSegmentPlane( At[i], At[j], nB, dB, p ) ) {
|
||||
// check if intersection lies inside triangle B
|
||||
@ -144,7 +144,7 @@ namespace {
|
||||
// clip edges of B against plane of A
|
||||
const pod::Vector3f Bt[3] = { B.points[0], B.points[1], B.points[2] };
|
||||
for ( auto i = 0; i < 3; ++i ) {
|
||||
int j = ( i + 1 ) % 3;
|
||||
auto j = ( i + 1 ) % 3;
|
||||
pod::Vector3f p;
|
||||
if ( intersectSegmentPlane( Bt[i], Bt[j], nA, dA, 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 ) {
|
||||
const auto& sphere = body;
|
||||
|
||||
float r = sphere.collider.u.sphere.radius;
|
||||
float r = sphere.collider.sphere.radius;
|
||||
auto center = ::getPosition( sphere );
|
||||
auto closest = ::closestPointOnTriangle( center, tri.points[0], tri.points[1], tri.points[2] );
|
||||
|
||||
@ -290,8 +290,8 @@ namespace {
|
||||
// to-do: implement
|
||||
bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) {
|
||||
const auto& plane = body;
|
||||
auto normal = plane.collider.u.plane.normal;
|
||||
float d = plane.collider.u.plane.offset;
|
||||
auto normal = plane.collider.plane.normal;
|
||||
float d = plane.collider.plane.offset;
|
||||
|
||||
bool hit = false;
|
||||
pod::Vector3f dist;
|
||||
@ -335,7 +335,7 @@ namespace {
|
||||
bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) {
|
||||
const auto& capsule = body;
|
||||
|
||||
float r = capsule.collider.u.capsule.radius;
|
||||
float r = capsule.collider.capsule.radius;
|
||||
auto [ p1, p2 ] = ::getCapsuleSegment( capsule );
|
||||
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 ) {
|
||||
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 ) {
|
||||
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 ) {
|
||||
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 ) {
|
||||
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 ) {
|
||||
ASSERT_COLLIDER_TYPES( TRIANGLE, CAPSULE );
|
||||
return ::triangleCapsule( a.collider.u.triangle, b, manifold, eps );
|
||||
return ::triangleCapsule( a.collider.triangle, b, manifold, eps );
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user