diff --git a/engine/inc/uf/utils/math/physics/common.h b/engine/inc/uf/utils/math/physics/common.h index d0207dd2..c4f37acc 100644 --- a/engine/inc/uf/utils/math/physics/common.h +++ b/engine/inc/uf/utils/math/physics/common.h @@ -81,6 +81,7 @@ namespace impl { pod::AABB transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ); /*FORCE_INLINE*/ bool aabbOverlap( const pod::qAABB& a, const pod::qAABB& b ); + /*FORCE_INLINE*/ pod::qAABB mergeAabb( const pod::qAABB& a, const pod::qAABB& b ); /*FORCE_INLINE*/ pod::qAABB quantizeAABB( const pod::AABB& box, const pod::AABB& root, const pod::Vector3f& invScale ); /*FORCE_INLINE*/ pod::AABB dequantizeAABB( const pod::qAABB& qbox, const pod::AABB& root ); /*FORCE_INLINE*/ pod::AABB dequantizeAABB( const pod::qAABB& qbox, const pod::AABB& root, const pod::Vector3f& scale ); diff --git a/engine/inc/uf/utils/memory/memcpy.h b/engine/inc/uf/utils/memory/memcpy.h index 2cfc0d80..4e2148cd 100644 --- a/engine/inc/uf/utils/memory/memcpy.h +++ b/engine/inc/uf/utils/memory/memcpy.h @@ -10,7 +10,7 @@ namespace uf { inline void* memcpy(void* dest, const void* src, size_t n) { #if UF_ENV_DREAMCAST if ( n >= 64 ) { - if ((dest & 31) == 0 && (n & 31) == 0 && (src & 3) == 0) { + if (((uintptr_t)(dest) & 31) == 0 && (n & 31) == 0 && ((uintptr_t)(src) & 3) == 0) { return ::sq_cpy(dest, src, n); } } diff --git a/engine/src/engine/graph/graph.cpp b/engine/src/engine/graph/graph.cpp index 71856723..c7e2e338 100644 --- a/engine/src/engine/graph/graph.cpp +++ b/engine/src/engine/graph/graph.cpp @@ -1715,8 +1715,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent ) else metadataJson["physics"] = phyziks; if ( ext::json::isObject( phyziks ) ) { - uf::stl::string type = phyziks["type"].as(); - + uf::stl::string type = phyziks["type"].as(); bool isMesh = type == "mesh" || type == "hull"; if ( !isMesh ) { if ( ext::json::isNull( metadataJson["physics"]["center"] ) ) metadataJson["physics"]["center"] = uf::vector::encode( bounds.center ); @@ -2222,27 +2221,33 @@ void uf::graph::reload( pod::Graph& graph, pod::Node& node ) { if ( ext::json::isObject( phyziks ) ) { uf::stl::string type = phyziks["type"].as(); bool isMesh = type == "mesh" || type == "hull"; - bool exists = entity.hasComponent(); - if ( isMesh ) { - auto& bvh = storage.bvhs.map[meshName]; - - // update bounds - if ( exists ) { - auto& body = entity.getComponent(); - uf::physics::update( body ); - } - if ( !exists ) { - // only need to initialize once, as the pointers will remain the same - float mass = phyziks["mass"].as(0.0f); + // cringe + if ( isMesh ) { + auto& bvh = storage.bvhs[meshName]; + float mass = phyziks["mass"].as(0.0f); + auto created = entity.hasComponent(); + auto& body = entity.getComponent(); + if ( !created ) { auto center = uf::vector::decode( phyziks["center"], pod::Vector3f{} ); - auto& body = uf::physics::create( entity, mass, center ); - + uf::physics::create( entity, mass, center ); + } + bool initialized = false; + // to-do: find a better initialization marker + switch ( body.collider.type ) { + case pod::ShapeType::MESH: + case pod::ShapeType::CONVEX_HULL: + initialized = true; + break; + } + if ( !initialized ) { uf::physics::initialize( body, mesh, bvh, type != "mesh" ); body.material.staticFriction = phyziks["friction"].as(body.material.staticFriction); body.material.restitution = phyziks["restitution"].as(body.material.restitution); body.inverseInertiaTensor = uf::vector::decode( phyziks["inertia"], body.inverseInertiaTensor ); body.gravity = uf::vector::decode( phyziks["gravity"], body.gravity ); + } else { + uf::physics::update( body ); } } } @@ -2614,7 +2619,7 @@ void uf::graph::reload( pod::Graph& graph ) { if ( bvhStream.buffer.length == 0 ) { rebuildBvh = true; } - bool bvhValid = !bvh.flatNodes.empty() || !bvh.nodes.empty(); + bool bvhValid = !bvh.indices.empty(); auto& indirectAttr = mesh.indirect.attributes.front(); pod::DrawCommand* drawCommands = (pod::DrawCommand*) mesh.buffers[indirectAttr.buffer].data(); diff --git a/engine/src/utils/math/physics/broadphase/bvh.cpp b/engine/src/utils/math/physics/broadphase/bvh.cpp index e3594512..11158c51 100644 --- a/engine/src/utils/math/physics/broadphase/bvh.cpp +++ b/engine/src/utils/math/physics/broadphase/bvh.cpp @@ -208,7 +208,8 @@ void impl::buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector& bodies, const pod::BVH::UpdatePolicy& policy, size_t frameCounter ) { // BVH is not built - if ( bvh.indices.empty() || bvh.nodes.empty() ) { + if ( bvh.indices.empty() || (bvh.nodes.empty() && bvh.flatNodes.empty()) ) { return pod::BVH::UpdatePolicy::Decision::REBUILD; } - if ( bodies.empty() ) return pod::BVH::UpdatePolicy::Decision::NONE; + if ( bodies.empty() ) { + return pod::BVH::UpdatePolicy::Decision::NONE; + } uint32_t dirtyCount = 0; float oldRootArea = impl::aabbSurfaceArea( bvh.rootBounds ); // update/check each body - for ( auto i = 0; i < bvh.nodes.size(); ++i ) { - auto& node = bvh.nodes[i]; - if ( /*node.count*/ node.getCount() == 0 ) continue; - auto& body = *bodies[bvh.indices[node.start]]; + if ( !bvh.flatNodes.empty() ) { + pod::Vector3f scale = impl::computeDequantizeScale( bvh.rootBounds ); + for ( auto i = 0; i < bvh.flatNodes.size(); ++i ) { + auto& node = bvh.flatNodes[i]; + if ( node.getCount() == 0 ) continue; - auto& oldBounds = bvh.bounds[i]; - auto& newBounds = body.bounds; + pod::AABB newLeafBounds = bodies[bvh.indices[node.start]]->bounds; + for ( pod::BVH::index_t j = 1; j < node.getCount(); ++j ) { + newLeafBounds = impl::mergeAabb(newLeafBounds, bodies[bvh.indices[node.start + j]]->bounds); + } - // compute displacement relative to size - pod::Vector3f oldCenter = impl::aabbCenter( oldBounds ); - pod::Vector3f newCenter = impl::aabbCenter( newBounds ); - float displacement = uf::vector::distance( newCenter, oldCenter ); + pod::AABB oldBounds = impl::dequantizeAABB( bvh.qBounds[i], bvh.rootBounds, scale ); - pod::Vector3f extent = oldBounds.max - oldBounds.min; - float size = std::max({extent.x, extent.y, extent.z, 1e-6f}); + pod::Vector3f oldCenter = impl::aabbCenter( oldBounds ); + pod::Vector3f newCenter = impl::aabbCenter( newLeafBounds ); + float displacement = uf::vector::distance( newCenter, oldCenter ); - if ( displacement > policy.displacementThreshold * size ) ++dirtyCount; + pod::Vector3f extent = oldBounds.max - oldBounds.min; + float size = std::max({extent.x, extent.y, extent.z, EPS}); + + if ( displacement > policy.displacementThreshold * size ) dirtyCount += node.getCount(); + } + } else { + for ( auto i = 0; i < bvh.nodes.size(); ++i ) { + auto& node = bvh.nodes[i]; + if ( /*node.count*/ node.getCount() == 0 ) continue; + auto& body = *bodies[bvh.indices[node.start]]; + + auto& oldBounds = bvh.bounds[i]; + auto& newBounds = body.bounds; + + // compute displacement relative to size + pod::Vector3f oldCenter = impl::aabbCenter( oldBounds ); + pod::Vector3f newCenter = impl::aabbCenter( newBounds ); + float displacement = uf::vector::distance( newCenter, oldCenter ); + + pod::Vector3f extent = oldBounds.max - oldBounds.min; + float size = std::max({extent.x, extent.y, extent.z, EPS}); + + if ( displacement > policy.displacementThreshold * size ) ++dirtyCount; + } } - float dirtyRatio = (float) dirtyCount / (float) bodies.size(); + float dirtyRatio = (float) dirtyCount / (float) bvh.indices.size(); // compute new root bounds pod::AABB newRoot = bodies[bvh.indices[0]]->bounds; @@ -351,78 +380,132 @@ pod::BVH::UpdatePolicy::Decision impl::decideBVHUpdate( pod::BVH& bvh, uf::stl:: 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; + + if ( dirtyCount > 0 ) { + return pod::BVH::UpdatePolicy::Decision::REFIT; + } return pod::BVH::UpdatePolicy::Decision::NONE; } void impl::refitBVH( pod::BVH& bvh, const uf::stl::vector& bounds ) { - if ( bvh.nodes.empty() ) return; + if ( bvh.nodes.empty() && bvh.flatNodes.empty() ) return; - // update leaf bounds - uf::stl::vector leaves; - leaves.reserve(uf::physics::settings.reserveCount); - for ( auto i = 0; i < bvh.nodes.size(); i++ ) { - if ( bvh.nodes[i].getCount() == 0 ) continue; - leaves.emplace_back(i); + pod::AABB newRoot = bounds[bvh.indices[0]]; + for ( auto i = 1; i < bvh.indices.size(); ++i ) { + newRoot = impl::mergeAabb(newRoot, bounds[bvh.indices[i]]); } + bvh.rootBounds = newRoot; - // recompute bounds from bodies - for ( auto i = 0; i < leaves.size(); i++ ) { - auto nodeID = leaves[i]; - auto& node = bvh.nodes[nodeID]; - auto& bound = bvh.bounds[nodeID]; - bound = bounds[bvh.indices[node.start]]; - for ( auto j = 1; j < node.getCount(); j++ ) - bound = impl::mergeAabb(bound, bounds[bvh.indices[node.start + j]]); - } + if ( !bvh.flatNodes.empty() ) { + pod::Vector3f invScale = impl::computeQuantizeScale(bvh.rootBounds); - // update internal nodes bottom-up - for ( int64_t i = (int64_t) bvh.nodes.size() - 1; i >= 0; i-- ) { - auto& node = bvh.nodes[i]; - auto& bound = bvh.bounds[i]; - // internal node - if ( node.getCount() == 0 ) { - bound = impl::mergeAabb(bvh.bounds[node.left], bvh.bounds[node.right]); + for ( int64_t i = (int64_t) bvh.flatNodes.size() - 1; i >= 0; i-- ) { + auto& node = bvh.flatNodes[i]; + auto& qbound = bvh.qBounds[i]; + + if ( node.getCount() > 0 ) { + pod::AABB floatBound = bounds[bvh.indices[node.start]]; + for ( auto j = 1; j < node.getCount(); j++ ) { + floatBound = impl::mergeAabb( floatBound, bounds[bvh.indices[node.start + j]] ); + } + qbound = impl::quantizeAABB(floatBound, bvh.rootBounds, invScale); + } else { + pod::BVH::index_t left = i + 1; + pod::BVH::index_t right = bvh.flatNodes[left].getSkipIndex(left); + + qbound = impl::mergeAabb( bvh.qBounds[left], bvh.qBounds[right] ); + } + } + } else { + uf::stl::vector leaves; + leaves.reserve(uf::physics::settings.reserveCount); + for ( auto i = 0; i < bvh.nodes.size(); i++ ) { + if ( bvh.nodes[i].getCount() > 0 ) leaves.emplace_back(i); } - } - if ( !bvh.flatNodes.empty() ) impl::flattenBVH( bvh, 0 ); + for ( auto i = 0; i < leaves.size(); i++ ) { + auto nodeID = leaves[i]; + auto& node = bvh.nodes[nodeID]; + auto& bound = bvh.bounds[nodeID]; + bound = bounds[bvh.indices[node.start]]; + for ( auto j = 1; j < node.getCount(); j++ ) + bound = impl::mergeAabb(bound, bounds[bvh.indices[node.start + j]]); + } + + for ( int64_t i = (int64_t) bvh.nodes.size() - 1; i >= 0; i-- ) { + auto& node = bvh.nodes[i]; + auto& bound = bvh.bounds[i]; + if ( node.getCount() == 0 ) { + bound = impl::mergeAabb(bvh.bounds[node.left], bvh.bounds[node.right]); + } + } + + bvh.rootBounds = bvh.bounds[0]; + } } // avoids creating a vector for bounds void impl::refitBVH( pod::BVH& bvh, const uf::stl::vector& bodies ) { - if ( bvh.nodes.empty() ) return; + if ( bvh.nodes.empty() && bvh.flatNodes.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.getCount() == 0 ) continue; - auto& bound = bvh.bounds[i]; - // leaf node: recompute bounds from bodies - auto nodeID = bvh.indices[node.start]; + pod::AABB newRoot = bodies[bvh.indices[0]]->bounds; + for ( auto i = 1; i < bvh.indices.size(); ++i ) { + newRoot = impl::mergeAabb(newRoot, bodies[bvh.indices[i]]->bounds); + } + bvh.rootBounds = newRoot; - bound = bodies[nodeID]->bounds; - node.setAsleep(!bodies[nodeID]->activity.awake); + if ( !bvh.flatNodes.empty() ) { + pod::Vector3f invScale = impl::computeQuantizeScale(bvh.rootBounds); - for ( auto j = 1; j < node.getCount(); j++ ) { - auto bodyID = bvh.indices[node.start + j]; - bound = impl::mergeAabb( bound, bodies[bodyID]->bounds ); - node.setAsleep(node.isAsleep() && !bodies[bodyID]->activity.awake); + for ( int64_t i = (int64_t) bvh.flatNodes.size() - 1; i >= 0; i-- ) { + auto& node = bvh.flatNodes[i]; + auto& qbound = bvh.qBounds[i]; + + if ( node.getCount() > 0 ) { + auto nodeID = bvh.indices[node.start]; + pod::AABB floatBound = bodies[nodeID]->bounds; + node.setAsleep(!bodies[nodeID]->activity.awake); + + for ( auto j = 1; j < node.getCount(); j++ ) { + auto bodyID = bvh.indices[node.start + j]; + floatBound = impl::mergeAabb( floatBound, bodies[bodyID]->bounds ); + node.setAsleep(node.isAsleep() && !bodies[bodyID]->activity.awake); + } + qbound = impl::quantizeAABB(floatBound, bvh.rootBounds, invScale); + } else { + pod::BVH::index_t left = i + 1; + pod::BVH::index_t right = bvh.flatNodes[left].getSkipIndex(left); + qbound = impl::mergeAabb( bvh.qBounds[left], bvh.qBounds[right] ); + node.setAsleep( bvh.flatNodes[left].isAsleep() && bvh.flatNodes[right].isAsleep() ); + } } - } + } else { + for ( auto i = 0; i < bvh.nodes.size(); i++ ) { + auto& node = bvh.nodes[i]; + if ( node.getCount() == 0 ) continue; - // update internal nodes bottom-up - for ( int64_t i = (int64_t) bvh.nodes.size() - 1; i >= 0; i-- ) { - auto& node = bvh.nodes[i]; - if ( node.getCount() > 0 ) continue; - // internal node - bvh.bounds[i] = impl::mergeAabb( bvh.bounds[node.left], bvh.bounds[node.right] ); - node.setAsleep( bvh.nodes[node.left].isAsleep() && bvh.nodes[node.right].isAsleep()); - } + auto& bound = bvh.bounds[i]; + auto nodeID = bvh.indices[node.start]; + bound = bodies[nodeID]->bounds; + node.setAsleep(!bodies[nodeID]->activity.awake); - if ( !bvh.flatNodes.empty() ) impl::flattenBVH( bvh, 0 ); + for ( auto j = 1; j < node.getCount(); j++ ) { + auto bodyID = bvh.indices[node.start + j]; + bound = impl::mergeAabb( bound, bodies[bodyID]->bounds ); + node.setAsleep(node.isAsleep() && !bodies[bodyID]->activity.awake); + } + } + + for ( int64_t i = (int64_t) bvh.nodes.size() - 1; i >= 0; i-- ) { + auto& node = bvh.nodes[i]; + if ( node.getCount() > 0 ) continue; + + bvh.bounds[i] = impl::mergeAabb( bvh.bounds[node.left], bvh.bounds[node.right] ); + node.setAsleep( bvh.nodes[node.left].isAsleep() && bvh.nodes[node.right].isAsleep()); + } + + bvh.rootBounds = bvh.bounds[0]; + } } void impl::refitBVH( pod::BVH& bvh, const uf::Mesh& mesh ) { @@ -1013,9 +1096,10 @@ size_t uf::bvh::serialize( const pod::BVH& bvh, uf::stl::vector& outBuf writer.write( (uint32_t)( bvh.indices.size() ) ); writer.write( (uint32_t)( bvh.nodes.size() ) ); writer.write( (uint32_t)( bvh.flatNodes.size() ) ); + writer.write( bvh.rootBounds ); if ( !bvh.indices.empty() ) writer.write( bvh.indices ); - if ( !bvh.nodes.empty() ) { writer.write( bvh.nodes ); writer.write( bvh.bounds); } + if ( !bvh.nodes.empty() ) { writer.write( bvh.nodes ); writer.write( bvh.bounds ); } if ( !bvh.flatNodes.empty() ) { writer.write( bvh.flatNodes ); writer.write( bvh.qBounds ); } return writer.offset() - offset; @@ -1025,15 +1109,17 @@ bool uf::bvh::deserialize( pod::BVH& bvh, const uf::stl::vector& buffer uf::stl::reader reader( buffer, offset, length > 0 ? length : buffer.size(), true, true ); const uint32_t* pNumIndices = reader.read(); - const uint32_t* pNumNodes = reader.read(); - const uint32_t* pNumFlat = reader.read(); + const uint32_t* pNumNodes = reader.read(); + const uint32_t* pNumFlat = reader.read(); + const pod::AABB* pRootBounds = reader.read(); - if ( !pNumIndices || !pNumNodes || !pNumFlat ) return false; + if ( !pNumIndices || !pNumNodes || !pNumFlat || !pRootBounds ) return false; uint32_t numIndices = *pNumIndices; uint32_t numNodes = *pNumNodes; uint32_t numFlat = *pNumFlat; + bvh.rootBounds = *pRootBounds; bvh.indices.clear(); bvh.nodes.clear(); bvh.bounds.clear(); @@ -1043,19 +1129,14 @@ bool uf::bvh::deserialize( pod::BVH& bvh, const uf::stl::vector& buffer if ( numIndices > 0 ) { if ( !reader.read( numIndices, bvh.indices ) ) return false; } - if ( numNodes > 0 ) { - // it "works", but sometimes unstable if ( numFlat > 0 ) { reader.skip( numNodes * sizeof(pod::BVH::Node) ); - reader.read( &bvh.rootBounds ); // read the first bounds as our root bounds - reader.skip( (numNodes - 1) * sizeof(pod::AABB) ); + reader.skip( numNodes * sizeof(pod::AABB) ); } else { if ( !reader.read( numNodes, bvh.nodes ) ) return false; - if ( !reader.read( numNodes, bvh.bounds ) ) return false; - - bvh.rootBounds = bvh.bounds[0]; // to-do: serialize this instead? + if ( !reader.read( numNodes, bvh.bounds ) ) return false; } } diff --git a/engine/src/utils/math/physics/common.cpp b/engine/src/utils/math/physics/common.cpp index 7f2caeb7..9819b1cf 100644 --- a/engine/src/utils/math/physics/common.cpp +++ b/engine/src/utils/math/physics/common.cpp @@ -839,6 +839,12 @@ uf::stl::string impl::getMaterialName( const pod::PhysicsBody& body, uint32_t pa bool impl::aabbOverlap( const pod::qAABB& a, const pod::qAABB& b ) { return (a.min <= b.max) && (a.max >= b.min); } +pod::qAABB impl::mergeAabb( const pod::qAABB& a, const pod::qAABB& b ) { + return { + uf::vector::min( a.min, b.min ), + uf::vector::max( a.max, b.max ), + }; +} pod::qAABB impl::quantizeAABB( const pod::AABB& box, const pod::AABB& root, const pod::Vector3f& invScale ) { pod::Vector3f min = (box.min - root.min) * invScale; pod::Vector3f max = (box.max - root.min) * invScale; diff --git a/engine/src/utils/math/physics/narrowphase/mesh.cpp b/engine/src/utils/math/physics/narrowphase/mesh.cpp index ef81e121..00f37f51 100644 --- a/engine/src/utils/math/physics/narrowphase/mesh.cpp +++ b/engine/src/utils/math/physics/narrowphase/mesh.cpp @@ -209,7 +209,6 @@ bool impl::meshHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod:: } void impl::drawMesh( const pod::PhysicsBody& body ) { - const uf::Mesh* meshData = body.collider.mesh.mesh; auto transform = impl::getTransform( body ); if ( !meshData ) return; @@ -217,9 +216,7 @@ void impl::drawMesh( const pod::PhysicsBody& body ) { if ( body.inverseMass == 0.0f ) { const auto& bvh = *body.collider.mesh.bvh; if ( !bvh.qBounds.empty() ) { - for ( const auto& qBounds : bvh.qBounds ) { - uf::debug::drawShape( impl::dequantizeAABB( qBounds, bvh.rootBounds ), transform ); - } + for ( const auto& qBounds : bvh.qBounds ) uf::debug::drawShape( impl::dequantizeAABB( qBounds, bvh.rootBounds ), transform ); return; } if ( !bvh.bounds.empty() ) {