732 lines
58 KiB
C++
732 lines
58 KiB
C++
#include "MetaCorePhysics/MetaCorePhysics.h"
|
|
|
|
#include "MetaCoreFoundation/MetaCoreHash.h"
|
|
#include "MetaCoreScene/MetaCoreScene.h"
|
|
#include "MetaCoreScene/MetaCoreTransformUtils.h"
|
|
|
|
#include <btBulletDynamicsCommon.h>
|
|
#include <BulletDynamics/ConstraintSolver/btFixedConstraint.h>
|
|
|
|
#include <algorithm>
|
|
#include <bit>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <limits>
|
|
#include <map>
|
|
#include <memory>
|
|
#include <tuple>
|
|
#include <unordered_map>
|
|
#include <utility>
|
|
|
|
namespace MetaCore {
|
|
namespace {
|
|
|
|
btVector3 Bt(glm::vec3 value) { return {value.x, value.y, value.z}; }
|
|
glm::vec3 Glm(const btVector3& value) { return {value.x(), value.y(), value.z()}; }
|
|
|
|
btTransform BuildTransform(const glm::mat4& matrix) {
|
|
glm::vec3 scale{}, translation{}, skew{}; glm::vec4 perspective{}; glm::quat rotation{};
|
|
glm::decompose(matrix, scale, rotation, translation, skew, perspective);
|
|
btTransform result; result.setOrigin(Bt(translation)); result.setRotation({rotation.x, rotation.y, rotation.z, rotation.w}); return result;
|
|
}
|
|
|
|
glm::mat4 BuildMatrix(const btTransform& transform, glm::vec3 scale) {
|
|
const btQuaternion value = transform.getRotation();
|
|
return glm::translate(glm::mat4(1.0F), Glm(transform.getOrigin())) *
|
|
glm::mat4_cast(glm::quat(value.w(), value.x(), value.y(), value.z())) * glm::scale(glm::mat4(1.0F), scale);
|
|
}
|
|
|
|
glm::mat4 WorldMatrix(const MetaCoreScene& scene, MetaCoreGameObject object) {
|
|
glm::mat4 result = MetaCoreBuildTransformMatrix(object.GetComponent<MetaCoreTransformComponent>());
|
|
MetaCoreId parent = object.GetParentId(); std::size_t guard = 0;
|
|
while (parent != 0 && guard++ < 1024U) {
|
|
const auto parentObject = scene.FindGameObject(parent); if (!parentObject) break;
|
|
result = MetaCoreBuildTransformMatrix(parentObject.GetComponent<MetaCoreTransformComponent>()) * result;
|
|
parent = parentObject.GetParentId();
|
|
}
|
|
return result;
|
|
}
|
|
|
|
struct PairKey {
|
|
MetaCoreId A = 0; MetaCoreId B = 0;
|
|
MetaCoreId ColliderA = 0; MetaCoreId ColliderB = 0;
|
|
bool Trigger = false;
|
|
auto operator<=>(const PairKey&) const = default;
|
|
};
|
|
|
|
} // namespace
|
|
|
|
class MetaCorePhysicsWorld::Impl {
|
|
public:
|
|
struct Metadata {
|
|
MetaCoreId Collider = 0;
|
|
MetaCoreId Body = 0;
|
|
std::uint32_t Layer = 0;
|
|
bool Trigger = false;
|
|
float Friction = 0.5F;
|
|
float Restitution = 0.0F;
|
|
MetaCorePhysicsMaterialCombineMode FrictionCombine = MetaCorePhysicsMaterialCombineMode::Average;
|
|
MetaCorePhysicsMaterialCombineMode RestitutionCombine = MetaCorePhysicsMaterialCombineMode::Average;
|
|
};
|
|
struct BodyEntry {
|
|
std::unique_ptr<btCollisionShape> Shape{};
|
|
std::vector<std::unique_ptr<btCollisionShape>> ChildShapes{};
|
|
std::vector<std::unique_ptr<btTriangleMesh>> TriangleMeshes{};
|
|
std::vector<std::unique_ptr<Metadata>> ColliderMetadata{};
|
|
std::unique_ptr<btDefaultMotionState> MotionState{};
|
|
std::unique_ptr<btRigidBody> Body{};
|
|
Metadata Meta{};
|
|
glm::vec3 Scale{1.0F};
|
|
glm::vec3 Center{0.0F};
|
|
MetaCoreRigidBodyType Type = MetaCoreRigidBodyType::Static;
|
|
bool Character = false;
|
|
float CharacterHalfHeight = 0.0F;
|
|
float CharacterSkinWidth = 0.05F;
|
|
float CharacterMaxFallSpeed = 55.0F;
|
|
float CharacterMaxSlopeDegrees = 45.0F;
|
|
float CharacterStepHeight = 0.3F;
|
|
float CharacterPushForce = 1.0F;
|
|
};
|
|
struct ConstraintEntry { MetaCoreId Owner = 0; MetaCoreId Target = 0; MetaCorePhysicsConstraintType Type = MetaCorePhysicsConstraintType::Fixed; std::unique_ptr<btTypedConstraint> Value{}; bool Broken = false; };
|
|
struct Filter final : btOverlapFilterCallback {
|
|
Impl* Owner = nullptr;
|
|
explicit Filter(Impl* owner) : Owner(owner) {}
|
|
bool needBroadphaseCollision(btBroadphaseProxy* lhs, btBroadphaseProxy* rhs) const override {
|
|
const auto* a = static_cast<const btCollisionObject*>(lhs->m_clientObject);
|
|
const auto* b = static_cast<const btCollisionObject*>(rhs->m_clientObject);
|
|
const auto* ma = a ? static_cast<const Metadata*>(a->getUserPointer()) : nullptr;
|
|
const auto* mb = b ? static_cast<const Metadata*>(b->getUserPointer()) : nullptr;
|
|
if (!ma || !mb || ma->Layer >= 32U || mb->Layer >= 32U) return true;
|
|
return ((Owner->Settings.CollisionMasks[ma->Layer] >> mb->Layer) & 1U) != 0U;
|
|
}
|
|
};
|
|
|
|
MetaCoreScene& Scene;
|
|
MetaCorePhysicsSettingsDocument Settings;
|
|
MetaCorePhysicsMeshProvider MeshProvider;
|
|
MetaCorePhysicsMaterialProvider MaterialProvider;
|
|
std::unique_ptr<btDefaultCollisionConfiguration> CollisionConfiguration = std::make_unique<btDefaultCollisionConfiguration>();
|
|
std::unique_ptr<btCollisionDispatcher> Dispatcher = std::make_unique<btCollisionDispatcher>(CollisionConfiguration.get());
|
|
std::unique_ptr<btDbvtBroadphase> Broadphase = std::make_unique<btDbvtBroadphase>();
|
|
std::unique_ptr<btSequentialImpulseConstraintSolver> Solver = std::make_unique<btSequentialImpulseConstraintSolver>();
|
|
std::unique_ptr<btDiscreteDynamicsWorld> World = std::make_unique<btDiscreteDynamicsWorld>(Dispatcher.get(), Broadphase.get(), Solver.get(), CollisionConfiguration.get());
|
|
Filter OverlapFilter{this};
|
|
std::unordered_map<MetaCoreId, std::unique_ptr<BodyEntry>> Bodies{};
|
|
std::vector<ConstraintEntry> Constraints{};
|
|
std::map<PairKey, MetaCorePhysicsContactEvent> PreviousPairs{};
|
|
MetaCorePhysicsEventCallback EventCallback{};
|
|
MetaCorePhysicsStatistics Statistics{};
|
|
std::vector<std::string> Diagnostics{};
|
|
std::vector<MetaCorePhysicsDebugLine> QueryDebugLines{};
|
|
std::unordered_map<std::string, MetaCorePhysicsMeshData> CollisionDerivedCache{};
|
|
std::uint64_t SceneRevision = std::numeric_limits<std::uint64_t>::max();
|
|
std::uint64_t StructureSignature = 0;
|
|
bool Stopped = false;
|
|
|
|
Impl(MetaCoreScene& scene, MetaCorePhysicsSettingsDocument settings, MetaCorePhysicsMeshProvider mesh, MetaCorePhysicsMaterialProvider material)
|
|
: Scene(scene), Settings(std::move(settings)), MeshProvider(std::move(mesh)), MaterialProvider(std::move(material)) {
|
|
(void)MetaCoreValidatePhysicsSettings(Settings, &Diagnostics);
|
|
gContactAddedCallback = &Impl::ContactAdded;
|
|
World->setGravity(Bt(Settings.Gravity));
|
|
World->getPairCache()->setOverlapFilterCallback(&OverlapFilter);
|
|
Rebuild();
|
|
}
|
|
|
|
~Impl() { Clear(); }
|
|
|
|
static int CombineRank(MetaCorePhysicsMaterialCombineMode mode) {
|
|
switch (mode) {
|
|
case MetaCorePhysicsMaterialCombineMode::Maximum: return 3;
|
|
case MetaCorePhysicsMaterialCombineMode::Multiply: return 2;
|
|
case MetaCorePhysicsMaterialCombineMode::Minimum: return 1;
|
|
case MetaCorePhysicsMaterialCombineMode::Average: return 0;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static float Combine(float lhs, float rhs, MetaCorePhysicsMaterialCombineMode lhsMode, MetaCorePhysicsMaterialCombineMode rhsMode) {
|
|
const auto mode = CombineRank(lhsMode) >= CombineRank(rhsMode) ? lhsMode : rhsMode;
|
|
switch (mode) {
|
|
case MetaCorePhysicsMaterialCombineMode::Maximum: return std::max(lhs, rhs);
|
|
case MetaCorePhysicsMaterialCombineMode::Multiply: return lhs * rhs;
|
|
case MetaCorePhysicsMaterialCombineMode::Minimum: return std::min(lhs, rhs);
|
|
case MetaCorePhysicsMaterialCombineMode::Average: return (lhs + rhs) * 0.5F;
|
|
}
|
|
return (lhs + rhs) * 0.5F;
|
|
}
|
|
|
|
static bool ContactAdded(btManifoldPoint& point, const btCollisionObjectWrapper* lhs, int, int,
|
|
const btCollisionObjectWrapper* rhs, int, int) {
|
|
const auto metadata=[](const btCollisionObjectWrapper* wrapper)->const Metadata*{if(!wrapper)return nullptr;if(wrapper->getCollisionShape()&&wrapper->getCollisionShape()->getUserPointer())return static_cast<const Metadata*>(wrapper->getCollisionShape()->getUserPointer());return static_cast<const Metadata*>(wrapper->getCollisionObject()->getUserPointer());};
|
|
const auto* a = metadata(lhs); const auto* b = metadata(rhs);
|
|
if (!a || !b) return true;
|
|
point.m_combinedFriction = std::clamp(Combine(a->Friction, b->Friction, a->FrictionCombine, b->FrictionCombine), -10.0F, 10.0F);
|
|
point.m_combinedRestitution = std::clamp(Combine(a->Restitution, b->Restitution, a->RestitutionCombine, b->RestitutionCombine), 0.0F, 1.0F);
|
|
return true;
|
|
}
|
|
|
|
void FlushExitEvents() {
|
|
if (EventCallback) for (const auto& [key, old] : PreviousPairs) {
|
|
auto event = old;
|
|
event.Type = key.Trigger ? MetaCorePhysicsEventType::TriggerExit : MetaCorePhysicsEventType::CollisionExit;
|
|
EventCallback(event);
|
|
}
|
|
PreviousPairs.clear();
|
|
}
|
|
|
|
void Clear() {
|
|
for (auto& entry : Constraints) if (entry.Value) World->removeConstraint(entry.Value.get());
|
|
Constraints.clear();
|
|
for (auto& [id, entry] : Bodies) { (void)id; if (entry->Body) World->removeRigidBody(entry->Body.get()); }
|
|
Bodies.clear(); PreviousPairs.clear();
|
|
}
|
|
|
|
std::uint64_t CalculateStructureSignature() const {
|
|
std::uint64_t hash = 1469598103934665603ULL;
|
|
const auto add = [&hash](std::uint64_t value) { hash ^= value; hash *= 1099511628211ULL; };
|
|
const auto addFloat = [&add](float value) { add(std::bit_cast<std::uint32_t>(value)); };
|
|
const auto addVec = [&addFloat](glm::vec3 value) { addFloat(value.x); addFloat(value.y); addFloat(value.z); };
|
|
for (const MetaCoreGameObject object : Scene.GetGameObjects()) {
|
|
add(object.GetId()); add(object.GetParentId()); addVec(object.GetComponent<MetaCoreTransformComponent>().Scale);
|
|
add(object.HasComponent<MetaCoreColliderComponent>()); add(object.HasComponent<MetaCoreRigidBodyComponent>());
|
|
add(object.HasComponent<MetaCorePhysicsConstraintComponent>()); add(object.HasComponent<MetaCoreCharacterControllerComponent>());
|
|
if (object.HasComponent<MetaCoreColliderComponent>()) {
|
|
const auto& value = object.GetComponent<MetaCoreColliderComponent>(); add(value.Enabled); add(static_cast<std::uint64_t>(value.Shape));
|
|
addVec(value.Center); addVec(value.Size); addFloat(value.Radius); addFloat(value.Height);
|
|
add(MetaCoreAssetGuidHasher{}(value.MeshAssetGuid)); add(MetaCoreAssetGuidHasher{}(value.MaterialAssetGuid)); add(value.CollisionLayer); add(value.IsTrigger);
|
|
}
|
|
if (object.HasComponent<MetaCoreRigidBodyComponent>()) {
|
|
const auto& value = object.GetComponent<MetaCoreRigidBodyComponent>(); add(static_cast<std::uint64_t>(value.BodyType)); addFloat(value.Mass);
|
|
addFloat(value.LinearDamping); addFloat(value.AngularDamping); addFloat(value.GravityScale); addVec(value.InitialLinearVelocity); addVec(value.InitialAngularVelocity);
|
|
add(value.AllowSleep); add(value.ContinuousCollisionDetection); add(value.LockedAxes);
|
|
}
|
|
if (object.HasComponent<MetaCorePhysicsConstraintComponent>()) {
|
|
const auto& value = object.GetComponent<MetaCorePhysicsConstraintComponent>(); add(value.Enabled); add(static_cast<std::uint64_t>(value.Type)); add(value.TargetObjectId);
|
|
addVec(value.Anchor); addVec(value.TargetAnchor); addVec(value.Axis); addVec(value.LinearLowerLimit); addVec(value.LinearUpperLimit); addVec(value.AngularLowerLimit); addVec(value.AngularUpperLimit);
|
|
add(value.MotorEnabled); addFloat(value.MotorTargetVelocity); addFloat(value.MotorMaxImpulse); addFloat(value.BreakForce); addFloat(value.BreakTorque); add(value.EnableConnectedCollision);
|
|
}
|
|
if (object.HasComponent<MetaCoreCharacterControllerComponent>()) {
|
|
const auto& value = object.GetComponent<MetaCoreCharacterControllerComponent>(); add(value.Enabled); addFloat(value.Radius); addFloat(value.Height); addFloat(value.SkinWidth);
|
|
addFloat(value.MaxSlopeDegrees); addFloat(value.StepHeight); addFloat(value.GravityScale); addFloat(value.MaxFallSpeed); addFloat(value.PushForce); add(value.CollisionLayer);
|
|
}
|
|
}
|
|
return hash;
|
|
}
|
|
|
|
std::unique_ptr<btCollisionShape> MakeShape(const MetaCoreColliderComponent& collider, glm::vec3 absoluteScale,
|
|
std::unique_ptr<btTriangleMesh>& triangleStorage, MetaCoreRigidBodyType bodyType, MetaCoreId objectId) {
|
|
const glm::vec3 size = glm::max(glm::abs(collider.Size * absoluteScale), glm::vec3(0.001F));
|
|
if (collider.Shape == MetaCoreColliderShape::Box) return std::make_unique<btBoxShape>(Bt(size * 0.5F));
|
|
if (collider.Shape == MetaCoreColliderShape::Sphere) return std::make_unique<btSphereShape>(std::max(0.001F, collider.Radius * std::max({absoluteScale.x, absoluteScale.y, absoluteScale.z})));
|
|
if (collider.Shape == MetaCoreColliderShape::Capsule) {
|
|
const float radius = std::max(0.001F, collider.Radius * std::max(absoluteScale.x, absoluteScale.y));
|
|
const float cylinderHeight = std::max(0.0F, collider.Height * absoluteScale.z - 2.0F * radius);
|
|
return std::make_unique<btCapsuleShapeZ>(radius, cylinderHeight);
|
|
}
|
|
if (!MeshProvider || !collider.MeshAssetGuid.IsValid()) { Diagnostics.push_back("对象 " + std::to_string(objectId) + " 缺少网格碰撞载荷"); return {}; }
|
|
const auto mesh = MeshProvider(collider.MeshAssetGuid);
|
|
if (!mesh || mesh->Positions.empty()) { Diagnostics.push_back("对象 " + std::to_string(objectId) + " 的网格碰撞载荷无效"); return {}; }
|
|
std::uint64_t payloadHash = MetaCoreHashBytes(std::as_bytes(std::span(mesh->Positions)));
|
|
payloadHash = MetaCoreHashCombine(payloadHash, MetaCoreHashBytes(std::as_bytes(std::span(mesh->Indices))));
|
|
const std::string cacheKey = MetaCoreBuildPhysicsCollisionCacheKey(collider.MeshAssetGuid, payloadHash, absoluteScale,
|
|
static_cast<std::uint32_t>(collider.Shape));
|
|
auto cached = CollisionDerivedCache.find(cacheKey);
|
|
if (cached == CollisionDerivedCache.end()) {
|
|
MetaCorePhysicsMeshData derived = *mesh;
|
|
for (glm::vec3& point : derived.Positions) point *= absoluteScale;
|
|
cached = CollisionDerivedCache.emplace(cacheKey, std::move(derived)).first;
|
|
}
|
|
const MetaCorePhysicsMeshData& collisionMesh = cached->second;
|
|
if (collider.Shape == MetaCoreColliderShape::ConvexHull) {
|
|
auto shape = std::make_unique<btConvexHullShape>();
|
|
for (glm::vec3 point : collisionMesh.Positions) shape->addPoint(Bt(point), false);
|
|
shape->recalcLocalAabb(); return shape;
|
|
}
|
|
if (bodyType != MetaCoreRigidBodyType::Static) { Diagnostics.push_back("对象 " + std::to_string(objectId) + " 的动态凹网格已被拒绝"); return {}; }
|
|
triangleStorage = std::make_unique<btTriangleMesh>();
|
|
for (std::size_t index = 0; index + 2U < collisionMesh.Indices.size(); index += 3U) {
|
|
const auto a = collisionMesh.Indices[index], b = collisionMesh.Indices[index + 1U], c = collisionMesh.Indices[index + 2U];
|
|
if (a < collisionMesh.Positions.size() && b < collisionMesh.Positions.size() && c < collisionMesh.Positions.size())
|
|
triangleStorage->addTriangle(Bt(collisionMesh.Positions[a]), Bt(collisionMesh.Positions[b]), Bt(collisionMesh.Positions[c]), true);
|
|
}
|
|
return std::make_unique<btBvhTriangleMeshShape>(triangleStorage.get(), true, true);
|
|
}
|
|
|
|
MetaCoreId ResolveBodyOwner(MetaCoreGameObject object) const {
|
|
if (object.HasComponent<MetaCoreRigidBodyComponent>() || object.HasComponent<MetaCoreCharacterControllerComponent>()) return object.GetId();
|
|
MetaCoreId parentId = object.GetParentId();
|
|
std::size_t guard = 0;
|
|
while (parentId != 0 && guard++ < 1024U) {
|
|
const MetaCoreGameObject parent = Scene.FindGameObject(parentId);
|
|
if (!parent) break;
|
|
if (parent.HasComponent<MetaCoreCharacterControllerComponent>()) break;
|
|
if (parent.HasComponent<MetaCoreRigidBodyComponent>()) return parent.GetId();
|
|
parentId = parent.GetParentId();
|
|
}
|
|
return object.GetId();
|
|
}
|
|
|
|
void Rebuild() {
|
|
FlushExitEvents(); Clear(); Diagnostics.clear(); SceneRevision = Scene.GetRevision(); StructureSignature = CalculateStructureSignature();
|
|
struct ColliderSource { MetaCoreGameObject Object{}; MetaCoreColliderComponent Collider{}; };
|
|
std::map<MetaCoreId, std::vector<ColliderSource>> groupedColliders;
|
|
for (MetaCoreGameObject object : Scene.GetGameObjects()) {
|
|
const bool character = object.HasComponent<MetaCoreCharacterControllerComponent>() && object.GetComponent<MetaCoreCharacterControllerComponent>().Enabled;
|
|
if (character && object.HasComponent<MetaCoreRigidBodyComponent>()) {
|
|
Diagnostics.push_back("对象 " + std::to_string(object.GetId()) + " 同时包含 CharacterController 与 RigidBody"); continue;
|
|
}
|
|
if (character) {
|
|
const auto& controller = object.GetComponent<MetaCoreCharacterControllerComponent>();
|
|
MetaCoreColliderComponent collider; collider.Shape=MetaCoreColliderShape::Capsule; collider.Radius=controller.Radius; collider.Height=controller.Height; collider.CollisionLayer=controller.CollisionLayer;
|
|
groupedColliders[object.GetId()].push_back({object, collider});
|
|
} else if (object.HasComponent<MetaCoreColliderComponent>() && object.GetComponent<MetaCoreColliderComponent>().Enabled) {
|
|
groupedColliders[ResolveBodyOwner(object)].push_back({object, object.GetComponent<MetaCoreColliderComponent>()});
|
|
}
|
|
}
|
|
for (auto& [ownerId, colliderSources] : groupedColliders) {
|
|
MetaCoreGameObject object = Scene.FindGameObject(ownerId); if (!object || colliderSources.empty()) continue;
|
|
const bool character = object.HasComponent<MetaCoreCharacterControllerComponent>() && object.GetComponent<MetaCoreCharacterControllerComponent>().Enabled;
|
|
auto bodyComponent = object.HasComponent<MetaCoreRigidBodyComponent>() ? object.GetComponent<MetaCoreRigidBodyComponent>() : MetaCoreRigidBodyComponent{};
|
|
if (character) { const auto& controller=object.GetComponent<MetaCoreCharacterControllerComponent>();bodyComponent.BodyType = MetaCoreRigidBodyType::Dynamic; bodyComponent.Mass = 80.0F; bodyComponent.AllowSleep = false;bodyComponent.GravityScale=controller.GravityScale; }
|
|
auto entry = std::make_unique<BodyEntry>(); entry->Type = bodyComponent.BodyType; entry->Center = glm::vec3(0.0F); entry->Character = character;
|
|
if (character) {
|
|
const auto& controller = object.GetComponent<MetaCoreCharacterControllerComponent>();
|
|
entry->CharacterHalfHeight = controller.Height * 0.5F;
|
|
entry->CharacterSkinWidth = controller.SkinWidth;
|
|
entry->CharacterMaxFallSpeed = controller.MaxFallSpeed;
|
|
entry->CharacterMaxSlopeDegrees = controller.MaxSlopeDegrees;
|
|
entry->CharacterStepHeight = controller.StepHeight;
|
|
entry->CharacterPushForce = controller.PushForce;
|
|
}
|
|
glm::vec3 scale{}, translation{}, skew{}; glm::vec4 perspective{}; glm::quat rotation{};
|
|
const glm::mat4 worldMatrix = WorldMatrix(Scene, object); glm::decompose(worldMatrix, scale, rotation, translation, skew, perspective);
|
|
const bool invalidScale = !std::isfinite(scale.x) || !std::isfinite(scale.y) || !std::isfinite(scale.z) ||
|
|
scale.x <= 1.0e-6F || scale.y <= 1.0e-6F || scale.z <= 1.0e-6F || glm::determinant(glm::mat3(worldMatrix)) <= 0.0F;
|
|
const bool sheared = glm::dot(skew, skew) > 1.0e-8F;
|
|
if (invalidScale || sheared) { Diagnostics.push_back("对象 " + std::to_string(object.GetId()) + " 的零/负缩放或父级非均匀变换无法用于物理"); continue; }
|
|
entry->Scale = scale;
|
|
const btTransform transform = BuildTransform(worldMatrix);
|
|
auto compound = std::make_unique<btCompoundShape>();
|
|
for (const ColliderSource& source : colliderSources) {
|
|
glm::vec3 childScale{}, childTranslation{}, childSkew{}; glm::vec4 childPerspective{}; glm::quat childRotation{};
|
|
const glm::mat4 childWorld = WorldMatrix(Scene, source.Object);
|
|
glm::decompose(childWorld, childScale, childRotation, childTranslation, childSkew, childPerspective);
|
|
const bool invalidChild = !std::isfinite(childScale.x) || !std::isfinite(childScale.y) || !std::isfinite(childScale.z) ||
|
|
childScale.x <= 1.0e-6F || childScale.y <= 1.0e-6F || childScale.z <= 1.0e-6F || glm::dot(childSkew, childSkew) > 1.0e-8F;
|
|
if (invalidChild) { Diagnostics.push_back("Collider 对象 " + std::to_string(source.Object.GetId()) + " 的变换无法聚合"); continue; }
|
|
std::unique_ptr<btTriangleMesh> triangle;
|
|
auto childShape = MakeShape(source.Collider, childScale, triangle, bodyComponent.BodyType, source.Object.GetId());
|
|
if (!childShape) continue;
|
|
auto metadata = std::make_unique<Metadata>(); metadata->Collider=source.Object.GetId(); metadata->Body=ownerId;
|
|
metadata->Layer=std::min(source.Collider.CollisionLayer,31U); metadata->Trigger=source.Collider.IsTrigger;
|
|
if (MaterialProvider && source.Collider.MaterialAssetGuid.IsValid()) if (auto material = MaterialProvider(source.Collider.MaterialAssetGuid)) {
|
|
metadata->Friction=std::max(0.0F,material->DynamicFriction); metadata->Restitution=std::clamp(material->Restitution,0.0F,1.0F);
|
|
metadata->FrictionCombine=material->FrictionCombine; metadata->RestitutionCombine=material->RestitutionCombine;
|
|
}
|
|
childShape->setUserPointer(metadata.get());
|
|
btTransform childTransform = transform.inverse() * BuildTransform(childWorld);
|
|
childTransform.setOrigin(childTransform.getOrigin() + childTransform.getBasis() * Bt(source.Collider.Center * childScale));
|
|
compound->addChildShape(childTransform, childShape.get());
|
|
entry->ColliderMetadata.push_back(std::move(metadata)); entry->ChildShapes.push_back(std::move(childShape));
|
|
if (triangle) entry->TriangleMeshes.push_back(std::move(triangle));
|
|
}
|
|
if (entry->ChildShapes.empty()) continue;
|
|
const bool mixedCompoundSemantics = std::any_of(entry->ColliderMetadata.begin() + 1, entry->ColliderMetadata.end(),
|
|
[&](const auto& metadata) {
|
|
return metadata->Layer != entry->ColliderMetadata.front()->Layer ||
|
|
metadata->Trigger != entry->ColliderMetadata.front()->Trigger;
|
|
});
|
|
if (mixedCompoundSemantics) {
|
|
Diagnostics.push_back("刚体对象 " + std::to_string(ownerId) + " 的聚合 Collider 必须使用相同碰撞层和 Trigger 设置");
|
|
continue;
|
|
}
|
|
entry->Shape = std::move(compound);
|
|
btScalar mass = bodyComponent.BodyType == MetaCoreRigidBodyType::Dynamic ? std::max(0.0001F, bodyComponent.Mass) : 0.0F;
|
|
btVector3 inertia{0.0F, 0.0F, 0.0F}; if (mass > 0.0F) entry->Shape->calculateLocalInertia(mass, inertia);
|
|
entry->MotionState = std::make_unique<btDefaultMotionState>(transform);
|
|
btRigidBody::btRigidBodyConstructionInfo info(mass, entry->MotionState.get(), entry->Shape.get(), inertia);
|
|
info.m_linearDamping = std::clamp(bodyComponent.LinearDamping, 0.0F, 1.0F); info.m_angularDamping = std::clamp(bodyComponent.AngularDamping, 0.0F, 1.0F);
|
|
entry->Body = std::make_unique<btRigidBody>(info);
|
|
entry->Meta = *entry->ColliderMetadata.front();
|
|
entry->Body->setUserPointer(&entry->Meta);
|
|
if (bodyComponent.BodyType == MetaCoreRigidBodyType::Kinematic) {
|
|
entry->Body->setCollisionFlags(entry->Body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); entry->Body->setActivationState(DISABLE_DEACTIVATION);
|
|
}
|
|
const bool allTriggers = std::all_of(entry->ColliderMetadata.begin(), entry->ColliderMetadata.end(), [](const auto& value) { return value->Trigger; });
|
|
if (allTriggers) entry->Body->setCollisionFlags(entry->Body->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
|
|
if (!bodyComponent.AllowSleep) entry->Body->setActivationState(DISABLE_DEACTIVATION);
|
|
if (bodyComponent.ContinuousCollisionDetection) { entry->Body->setCcdMotionThreshold(0.001F); entry->Body->setCcdSweptSphereRadius(0.1F); }
|
|
entry->Body->setLinearVelocity(Bt(bodyComponent.InitialLinearVelocity)); entry->Body->setAngularVelocity(Bt(bodyComponent.InitialAngularVelocity));
|
|
entry->Body->setGravity(Bt(Settings.Gravity * bodyComponent.GravityScale));
|
|
if (character) entry->Body->setAngularFactor(btVector3(0.0F, 0.0F, 0.0F));
|
|
entry->Body->setFriction(entry->Meta.Friction); entry->Body->setRestitution(entry->Meta.Restitution);
|
|
entry->Body->setCollisionFlags(entry->Body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
|
|
const btVector3 linearFactor{
|
|
(bodyComponent.LockedAxes & (1U << 0U)) ? 0.0F : 1.0F,
|
|
(bodyComponent.LockedAxes & (1U << 1U)) ? 0.0F : 1.0F,
|
|
(bodyComponent.LockedAxes & (1U << 2U)) ? 0.0F : 1.0F};
|
|
const btVector3 angularFactor{
|
|
(bodyComponent.LockedAxes & (1U << 3U)) ? 0.0F : 1.0F,
|
|
(bodyComponent.LockedAxes & (1U << 4U)) ? 0.0F : 1.0F,
|
|
(bodyComponent.LockedAxes & (1U << 5U)) ? 0.0F : 1.0F};
|
|
entry->Body->setLinearFactor(linearFactor); entry->Body->setAngularFactor(character ? btVector3(0.0F, 0.0F, 0.0F) : angularFactor);
|
|
World->addRigidBody(entry->Body.get()); Bodies.emplace(ownerId, std::move(entry));
|
|
}
|
|
for (MetaCoreGameObject object : Scene.GetGameObjects()) {
|
|
if (!object.HasComponent<MetaCorePhysicsConstraintComponent>()) continue;
|
|
const auto& source = object.GetComponent<MetaCorePhysicsConstraintComponent>(); if (!source.Enabled) continue;
|
|
const auto owner = Bodies.find(object.GetId()); if (owner == Bodies.end()) { Diagnostics.push_back("约束对象 " + std::to_string(object.GetId()) + " 缺少刚体"); continue; }
|
|
auto target = source.TargetObjectId == 0 ? nullptr : (Bodies.contains(source.TargetObjectId) ? Bodies.at(source.TargetObjectId).get() : nullptr);
|
|
if (source.TargetObjectId != 0 && !target) { Diagnostics.push_back("约束对象 " + std::to_string(object.GetId()) + " 的目标刚体不存在"); continue; }
|
|
btRigidBody& a = *owner->second->Body; btRigidBody& b = target ? *target->Body : btTypedConstraint::getFixedBody();
|
|
btTransform frameA, frameB; frameA.setIdentity(); frameB.setIdentity(); frameA.setOrigin(Bt(source.Anchor)); frameB.setOrigin(Bt(source.TargetAnchor));
|
|
const glm::vec3 normalizedAxis = glm::dot(source.Axis,source.Axis)>1.0e-8F?glm::normalize(source.Axis):glm::vec3(1.0F,0.0F,0.0F);
|
|
const btQuaternion axisRotation = shortestArcQuat(btVector3(1.0F,0.0F,0.0F),Bt(normalizedAxis));
|
|
frameA.setRotation(axisRotation); frameB.setRotation(axisRotation);
|
|
std::unique_ptr<btTypedConstraint> constraint;
|
|
if (source.Type == MetaCorePhysicsConstraintType::Fixed) constraint = std::make_unique<btFixedConstraint>(a, b, frameA, frameB);
|
|
else if (source.Type == MetaCorePhysicsConstraintType::Hinge) {
|
|
auto value = std::make_unique<btHingeConstraint>(a, b, frameA, frameB); value->setLimit(glm::radians(source.AngularLowerLimit.x), glm::radians(source.AngularUpperLimit.x));
|
|
if (source.MotorEnabled) value->enableAngularMotor(true, source.MotorTargetVelocity, source.MotorMaxImpulse); constraint = std::move(value);
|
|
} else if (source.Type == MetaCorePhysicsConstraintType::Slider) {
|
|
auto value = std::make_unique<btSliderConstraint>(a, b, frameA, frameB, true); value->setLowerLinLimit(source.LinearLowerLimit.x); value->setUpperLinLimit(source.LinearUpperLimit.x);
|
|
if (source.MotorEnabled) { value->setPoweredLinMotor(true); value->setTargetLinMotorVelocity(source.MotorTargetVelocity); value->setMaxLinMotorForce(source.MotorMaxImpulse); } constraint = std::move(value);
|
|
} else {
|
|
auto value = std::make_unique<btGeneric6DofConstraint>(a, b, frameA, frameB, true); value->setLinearLowerLimit(Bt(source.LinearLowerLimit)); value->setLinearUpperLimit(Bt(source.LinearUpperLimit));
|
|
value->setAngularLowerLimit(Bt(glm::radians(source.AngularLowerLimit))); value->setAngularUpperLimit(Bt(glm::radians(source.AngularUpperLimit))); constraint = std::move(value);
|
|
}
|
|
const float breakLimit = source.BreakForce > 0.0F && source.BreakTorque > 0.0F ? std::min(source.BreakForce,source.BreakTorque) : std::max(source.BreakForce,source.BreakTorque);
|
|
if (breakLimit > 0.0F) constraint->setBreakingImpulseThreshold(breakLimit * static_cast<float>(Settings.FixedTimeStep));
|
|
World->addConstraint(constraint.get(), !source.EnableConnectedCollision); Constraints.push_back({object.GetId(), source.TargetObjectId, source.Type, std::move(constraint), false});
|
|
}
|
|
Statistics.BodyCount = Bodies.size();
|
|
}
|
|
|
|
bool Accept(const Metadata* metadata, const MetaCorePhysicsQueryFilter& filter) const {
|
|
return metadata && metadata->Layer < 32U && ((filter.LayerMask >> metadata->Layer) & 1U) != 0U &&
|
|
!(metadata->Trigger && filter.Triggers == MetaCorePhysicsTriggerQuery::Ignore) &&
|
|
!filter.IgnoredObjectIds.contains(metadata->Collider) && !filter.IgnoredBodyIds.contains(metadata->Body);
|
|
}
|
|
|
|
const Metadata* ResolveMetadata(const btCollisionObject* object, const btCollisionWorld::LocalShapeInfo* shapeInfo) const {
|
|
const auto* bodyMetadata = object ? static_cast<const Metadata*>(object->getUserPointer()) : nullptr;
|
|
if (!bodyMetadata || !shapeInfo || shapeInfo->m_shapePart < 0) return bodyMetadata;
|
|
const auto body = Bodies.find(bodyMetadata->Body);
|
|
const std::size_t childIndex = static_cast<std::size_t>(shapeInfo->m_shapePart);
|
|
return body != Bodies.end() && childIndex < body->second->ColliderMetadata.size() ? body->second->ColliderMetadata[childIndex].get() : bodyMetadata;
|
|
}
|
|
const Metadata* ResolveContactMetadata(const btCollisionObject* object, int partId, int childIndex) const {
|
|
const auto* bodyMetadata=object?static_cast<const Metadata*>(object->getUserPointer()):nullptr;if(!bodyMetadata)return nullptr;
|
|
const int resolvedIndex=partId>=0?partId:childIndex;if(resolvedIndex<0)return bodyMetadata;const auto body=Bodies.find(bodyMetadata->Body);
|
|
const std::size_t index=static_cast<std::size_t>(resolvedIndex);return body!=Bodies.end()&&index<body->second->ColliderMetadata.size()?body->second->ColliderMetadata[index].get():bodyMetadata;
|
|
}
|
|
const Metadata* ResolveMetadataAtPoint(const btCollisionObject* object, const btVector3& point) const {
|
|
const auto* bodyMetadata=object?static_cast<const Metadata*>(object->getUserPointer()):nullptr;if(!bodyMetadata)return nullptr;const auto body=Bodies.find(bodyMetadata->Body);
|
|
if(body==Bodies.end()||body->second->ColliderMetadata.size()<2U)return bodyMetadata;const auto* compound=dynamic_cast<const btCompoundShape*>(object->getCollisionShape());if(!compound)return bodyMetadata;
|
|
const Metadata* closest=bodyMetadata;btScalar closestDistance=SIMD_INFINITY;
|
|
for(int index=0;index<compound->getNumChildShapes()&&static_cast<std::size_t>(index)<body->second->ColliderMetadata.size();++index){btVector3 minimum,maximum;compound->getChildShape(index)->getAabb(object->getWorldTransform()*compound->getChildTransform(index),minimum,maximum);const btVector3 clamped(btClamped(point.x(),minimum.x(),maximum.x()),btClamped(point.y(),minimum.y(),maximum.y()),btClamped(point.z(),minimum.z(),maximum.z()));const btScalar distance=(point-clamped).length2();if(distance<closestDistance){closestDistance=distance;closest=body->second->ColliderMetadata[static_cast<std::size_t>(index)].get();}}
|
|
return closest;
|
|
}
|
|
|
|
MetaCorePhysicsHit Hit(const btCollisionObject* object, const btVector3& position, const btVector3& normal, float distance, float fraction,
|
|
const Metadata* resolvedMetadata = nullptr) const {
|
|
const auto* metadata = resolvedMetadata ? resolvedMetadata : object ? static_cast<const Metadata*>(object->getUserPointer()) : nullptr;
|
|
return {metadata ? metadata->Collider : 0, metadata ? metadata->Body : 0, Glm(position), Glm(normal), distance, fraction, metadata && metadata->Trigger};
|
|
}
|
|
|
|
void EmitContacts() {
|
|
std::map<PairKey, MetaCorePhysicsContactEvent> current;
|
|
for (int index = 0; index < Dispatcher->getNumManifolds(); ++index) {
|
|
const btPersistentManifold* manifold = Dispatcher->getManifoldByIndexInternal(index); if (!manifold || manifold->getNumContacts() == 0) continue;
|
|
const btManifoldPoint& point = manifold->getContactPoint(0); if (point.getDistance() > 0.0F) continue;
|
|
const auto* a = ResolveContactMetadata(static_cast<const btCollisionObject*>(manifold->getBody0()),point.m_partId0,point.m_index0);
|
|
const auto* b = ResolveContactMetadata(static_cast<const btCollisionObject*>(manifold->getBody1()),point.m_partId1,point.m_index1); if (!a || !b) continue;
|
|
const bool trigger = a->Trigger || b->Trigger; const bool swapped = a->Body > b->Body;
|
|
PairKey key{std::min(a->Body, b->Body), std::max(a->Body, b->Body), swapped?b->Collider:a->Collider, swapped?a->Collider:b->Collider, trigger};
|
|
MetaCorePhysicsContactEvent event; event.Type = trigger ? MetaCorePhysicsEventType::TriggerStay : MetaCorePhysicsEventType::CollisionStay;
|
|
event.ObjectA = swapped ? b->Body : a->Body; event.ObjectB = swapped ? a->Body : b->Body;
|
|
event.ColliderA = swapped ? b->Collider : a->Collider; event.ColliderB = swapped ? a->Collider : b->Collider;
|
|
event.Point = Glm(point.getPositionWorldOnB()); event.Normal = Glm(swapped ? -point.m_normalWorldOnB : point.m_normalWorldOnB); event.Impulse = point.getAppliedImpulse();
|
|
const auto* bodyA = btRigidBody::upcast(manifold->getBody0()); const auto* bodyB = btRigidBody::upcast(manifold->getBody1());
|
|
const btVector3 relativeVelocity = (bodyB ? bodyB->getLinearVelocity() : btVector3{}) - (bodyA ? bodyA->getLinearVelocity() : btVector3{});
|
|
event.RelativeVelocity = Glm(swapped ? -relativeVelocity : relativeVelocity);
|
|
current.insert_or_assign(key, event);
|
|
}
|
|
for (auto& [key, event] : current) {
|
|
if (!PreviousPairs.contains(key)) event.Type = key.Trigger ? MetaCorePhysicsEventType::TriggerEnter : MetaCorePhysicsEventType::CollisionEnter;
|
|
if (EventCallback) EventCallback(event);
|
|
}
|
|
for (const auto& [key, old] : PreviousPairs) if (!current.contains(key) && EventCallback) {
|
|
auto event = old; event.Type = key.Trigger ? MetaCorePhysicsEventType::TriggerExit : MetaCorePhysicsEventType::CollisionExit; EventCallback(event);
|
|
}
|
|
PreviousPairs = std::move(current); Statistics.ContactCount = PreviousPairs.size();
|
|
for (auto& entry : Constraints) if (!entry.Broken && entry.Value && !entry.Value->isEnabled()) {
|
|
entry.Broken = true; if (EventCallback) { MetaCorePhysicsContactEvent event; event.Type=MetaCorePhysicsEventType::ConstraintBroken; event.ObjectA=entry.Owner; event.ObjectB=entry.Target; EventCallback(event); }
|
|
}
|
|
}
|
|
|
|
void SyncKinematicAndStatic() {
|
|
for (auto& [id, entry] : Bodies) {
|
|
if (entry->Type == MetaCoreRigidBodyType::Dynamic) continue;
|
|
const auto object = Scene.FindGameObject(id); if (!object) continue;
|
|
btTransform transform = BuildTransform(WorldMatrix(Scene, object));
|
|
transform.setOrigin(transform.getOrigin() + transform.getBasis() * Bt(entry->Center * entry->Scale));
|
|
entry->Body->setWorldTransform(transform); entry->MotionState->setWorldTransform(transform); entry->Body->activate(true);
|
|
}
|
|
}
|
|
|
|
void WriteBack() {
|
|
for (auto& [id, entry] : Bodies) {
|
|
if (entry->Type != MetaCoreRigidBodyType::Dynamic) continue;
|
|
auto object = Scene.FindGameObject(id); if (!object) continue;
|
|
btTransform transform; entry->MotionState->getWorldTransform(transform);
|
|
transform.setOrigin(transform.getOrigin() - transform.getBasis() * Bt(entry->Center * entry->Scale));
|
|
glm::mat4 local = BuildMatrix(transform, entry->Scale);
|
|
if (object.GetParentId() != 0) if (const auto parent = Scene.FindGameObject(object.GetParentId())) local = glm::inverse(WorldMatrix(Scene, parent)) * local;
|
|
MetaCoreApplyMatrixToTransform(local, object.GetComponent<MetaCoreTransformComponent>());
|
|
}
|
|
}
|
|
};
|
|
|
|
MetaCorePhysicsWorld::MetaCorePhysicsWorld(MetaCoreScene& scene, MetaCorePhysicsSettingsDocument settings,
|
|
MetaCorePhysicsMeshProvider meshProvider, MetaCorePhysicsMaterialProvider materialProvider)
|
|
: Impl_(std::make_unique<Impl>(scene, std::move(settings), std::move(meshProvider), std::move(materialProvider))) {}
|
|
MetaCorePhysicsWorld::~MetaCorePhysicsWorld() = default;
|
|
void MetaCorePhysicsWorld::SynchronizeScene() {
|
|
const auto signature = Impl_->CalculateStructureSignature();
|
|
if (signature != Impl_->StructureSignature) Impl_->Rebuild();
|
|
else { Impl_->SceneRevision = Impl_->Scene.GetRevision(); Impl_->SyncKinematicAndStatic(); }
|
|
}
|
|
void MetaCorePhysicsWorld::SetEventCallback(MetaCorePhysicsEventCallback callback) { Impl_->EventCallback = std::move(callback); }
|
|
void MetaCorePhysicsWorld::Step(double fixedDeltaSeconds) {
|
|
if (Impl_->Stopped || !std::isfinite(fixedDeltaSeconds) || fixedDeltaSeconds <= 0.0) return;
|
|
const auto begin = std::chrono::steady_clock::now(); SynchronizeScene();
|
|
for (auto& [id, entry] : Impl_->Bodies) {
|
|
(void)id;
|
|
if (!entry->Character) continue;
|
|
btVector3 velocity = entry->Body->getLinearVelocity();
|
|
velocity.setZ(std::max(velocity.z(), -entry->CharacterMaxFallSpeed));
|
|
entry->Body->setLinearVelocity(velocity);
|
|
}
|
|
Impl_->World->stepSimulation(static_cast<btScalar>(fixedDeltaSeconds), 0);
|
|
Impl_->WriteBack(); Impl_->EmitContacts(); Impl_->Statistics.LastStepMilliseconds = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - begin).count();
|
|
}
|
|
void MetaCorePhysicsWorld::Stop() {
|
|
if (Impl_->Stopped) return;
|
|
Impl_->FlushExitEvents();
|
|
Impl_->Stopped = true; Impl_->Clear();
|
|
}
|
|
|
|
std::vector<MetaCorePhysicsHit> MetaCorePhysicsWorld::RaycastAll(glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) {
|
|
++Impl_->Statistics.QueryCount; std::vector<MetaCorePhysicsHit> result;
|
|
if (!std::isfinite(maxDistance) || maxDistance <= 0.0F || glm::dot(direction, direction) < 1.0e-12F) return result;
|
|
direction = glm::normalize(direction); const btVector3 from = Bt(origin), to = Bt(origin + direction * maxDistance);
|
|
Impl_->QueryDebugLines = {{origin, origin + direction * maxDistance, glm::vec3(0.2F, 0.8F, 1.0F)}};
|
|
struct Callback final : btCollisionWorld::AllHitsRayResultCallback {
|
|
Impl* Owner; const MetaCorePhysicsQueryFilter& Filter; std::vector<const Impl::Metadata*> MetadataHits{};
|
|
Callback(const btVector3& a, const btVector3& b, Impl* owner, const MetaCorePhysicsQueryFilter& filter) : AllHitsRayResultCallback(a, b), Owner(owner), Filter(filter) {}
|
|
bool needsCollision(btBroadphaseProxy* proxy) const override { const auto* object = static_cast<const btCollisionObject*>(proxy->m_clientObject); return Owner->Accept(object ? static_cast<const Impl::Metadata*>(object->getUserPointer()) : nullptr, Filter); }
|
|
btScalar addSingleResult(btCollisionWorld::LocalRayResult& value, bool normalInWorldSpace) override {
|
|
MetadataHits.push_back(Owner->ResolveMetadata(value.m_collisionObject,value.m_localShapeInfo));
|
|
return AllHitsRayResultCallback::addSingleResult(value,normalInWorldSpace);
|
|
}
|
|
} callback(from, to, Impl_.get(), filter);
|
|
Impl_->World->rayTest(from, to, callback);
|
|
for (int index = 0; index < callback.m_collisionObjects.size(); ++index) result.push_back(Impl_->Hit(callback.m_collisionObjects[index], callback.m_hitPointWorld[index], callback.m_hitNormalWorld[index], maxDistance * callback.m_hitFractions[index], callback.m_hitFractions[index], Impl_->ResolveMetadataAtPoint(callback.m_collisionObjects[index],callback.m_hitPointWorld[index])));
|
|
std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { return std::tie(a.Fraction, a.ColliderObjectId, a.RigidBodyObjectId) < std::tie(b.Fraction, b.ColliderObjectId, b.RigidBodyObjectId); });
|
|
if (!result.empty()) Impl_->QueryDebugLines.push_back({result.front().Position, result.front().Position + result.front().Normal * 0.35F, glm::vec3(1.0F, 0.25F, 0.2F)});
|
|
return result;
|
|
}
|
|
std::optional<MetaCorePhysicsHit> MetaCorePhysicsWorld::Raycast(glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) { auto all = RaycastAll(origin, direction, maxDistance, filter); return all.empty() ? std::nullopt : std::optional<MetaCorePhysicsHit>(all.front()); }
|
|
|
|
static std::unique_ptr<btConvexShape> MetaCoreMakeQueryShape(const MetaCorePhysicsQueryShape& shape) {
|
|
if (shape.Type == MetaCorePhysicsQueryShapeType::Sphere && shape.Radius > 0.0F) return std::make_unique<btSphereShape>(shape.Radius);
|
|
if (shape.Type == MetaCorePhysicsQueryShapeType::Capsule && shape.Radius > 0.0F && shape.Height >= shape.Radius * 2.0F) return std::make_unique<btCapsuleShapeZ>(shape.Radius, shape.Height - shape.Radius * 2.0F);
|
|
if (shape.Type == MetaCorePhysicsQueryShapeType::Box && shape.HalfExtents.x > 0.0F && shape.HalfExtents.y > 0.0F && shape.HalfExtents.z > 0.0F) return std::make_unique<btBoxShape>(Bt(shape.HalfExtents));
|
|
return {};
|
|
}
|
|
|
|
std::vector<MetaCorePhysicsHit> MetaCorePhysicsWorld::ShapeCastAll(const MetaCorePhysicsQueryShape& shape, glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) {
|
|
++Impl_->Statistics.QueryCount; std::vector<MetaCorePhysicsHit> result; auto queryShape = MetaCoreMakeQueryShape(shape);
|
|
if (!queryShape || maxDistance <= 0.0F || glm::dot(direction, direction) < 1.0e-12F) return result;
|
|
direction = glm::normalize(direction);
|
|
btTransform from, to; from.setIdentity(); to.setIdentity(); from.setOrigin(Bt(origin)); to.setOrigin(Bt(origin + direction * maxDistance));
|
|
struct Callback final : btCollisionWorld::ClosestConvexResultCallback {
|
|
Impl* Owner; const MetaCorePhysicsQueryFilter& Filter; std::vector<MetaCorePhysicsHit>& Hits; float Distance;
|
|
Callback(const btVector3& a, const btVector3& b, Impl* owner, const MetaCorePhysicsQueryFilter& filter, std::vector<MetaCorePhysicsHit>& hits, float distance)
|
|
: ClosestConvexResultCallback(a, b), Owner(owner), Filter(filter), Hits(hits), Distance(distance) { m_closestHitFraction = 1.0F; }
|
|
bool needsCollision(btBroadphaseProxy* proxy) const override { const auto* object = static_cast<const btCollisionObject*>(proxy->m_clientObject); return Owner->Accept(object ? static_cast<const Impl::Metadata*>(object->getUserPointer()) : nullptr, Filter); }
|
|
btScalar addSingleResult(btCollisionWorld::LocalConvexResult& value, bool normalInWorldSpace) override {
|
|
const btVector3 normal = normalInWorldSpace ? value.m_hitNormalLocal : value.m_hitCollisionObject->getWorldTransform().getBasis() * value.m_hitNormalLocal;
|
|
Hits.push_back(Owner->Hit(value.m_hitCollisionObject, value.m_hitPointLocal, normal, Distance * value.m_hitFraction, value.m_hitFraction, Owner->ResolveMetadataAtPoint(value.m_hitCollisionObject,value.m_hitPointLocal))); return 1.0F;
|
|
}
|
|
} callback(from.getOrigin(), to.getOrigin(), Impl_.get(), filter, result, maxDistance);
|
|
Impl_->World->convexSweepTest(queryShape.get(), from, to, callback);
|
|
std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { return std::tie(a.Fraction, a.ColliderObjectId, a.RigidBodyObjectId) < std::tie(b.Fraction, b.ColliderObjectId, b.RigidBodyObjectId); }); return result;
|
|
}
|
|
std::optional<MetaCorePhysicsHit> MetaCorePhysicsWorld::ShapeCast(const MetaCorePhysicsQueryShape& shape, glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) { auto all = ShapeCastAll(shape, origin, direction, maxDistance, filter); return all.empty() ? std::nullopt : std::optional<MetaCorePhysicsHit>(all.front()); }
|
|
|
|
std::vector<MetaCorePhysicsHit> MetaCorePhysicsWorld::Overlap(const MetaCorePhysicsQueryShape& shape, glm::vec3 center, const MetaCorePhysicsQueryFilter& filter) {
|
|
++Impl_->Statistics.QueryCount; std::vector<MetaCorePhysicsHit> result; auto queryShape = MetaCoreMakeQueryShape(shape); if (!queryShape) return result;
|
|
btCollisionObject query; query.setCollisionShape(queryShape.get()); btTransform transform; transform.setIdentity(); transform.setOrigin(Bt(center)); query.setWorldTransform(transform);
|
|
struct Callback final : btCollisionWorld::ContactResultCallback {
|
|
Impl* Owner; const MetaCorePhysicsQueryFilter& Filter; std::vector<MetaCorePhysicsHit>& Hits; const btCollisionObject* Query;
|
|
Callback(Impl* owner, const MetaCorePhysicsQueryFilter& filter, std::vector<MetaCorePhysicsHit>& hits, const btCollisionObject* query) : Owner(owner), Filter(filter), Hits(hits), Query(query) {}
|
|
bool needsCollision(btBroadphaseProxy* proxy) const override { const auto* object = static_cast<const btCollisionObject*>(proxy->m_clientObject); return Owner->Accept(object ? static_cast<const Impl::Metadata*>(object->getUserPointer()) : nullptr, Filter); }
|
|
btScalar addSingleResult(btManifoldPoint& point, const btCollisionObjectWrapper* a, int, int, const btCollisionObjectWrapper* b, int, int) override {
|
|
const btCollisionObject* object = a->getCollisionObject() == Query ? b->getCollisionObject() : a->getCollisionObject();
|
|
Hits.push_back(Owner->Hit(object, point.getPositionWorldOnB(), point.m_normalWorldOnB, 0.0F, 0.0F)); return 0.0F;
|
|
}
|
|
} callback(Impl_.get(), filter, result, &query);
|
|
Impl_->World->contactTest(&query, callback);
|
|
std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { return std::tie(a.ColliderObjectId, a.RigidBodyObjectId) < std::tie(b.ColliderObjectId, b.RigidBodyObjectId); });
|
|
result.erase(std::unique(result.begin(), result.end(), [](const auto& a, const auto& b) { return a.ColliderObjectId == b.ColliderObjectId && a.RigidBodyObjectId == b.RigidBodyObjectId; }), result.end()); return result;
|
|
}
|
|
|
|
bool MetaCorePhysicsWorld::AddForce(MetaCoreId id, glm::vec3 value) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || it->second->Type != MetaCoreRigidBodyType::Dynamic) return false; it->second->Body->activate(true); it->second->Body->applyCentralForce(Bt(value)); return true; }
|
|
bool MetaCorePhysicsWorld::AddImpulse(MetaCoreId id, glm::vec3 value) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || it->second->Type != MetaCoreRigidBodyType::Dynamic) return false; it->second->Body->activate(true); it->second->Body->applyCentralImpulse(Bt(value)); return true; }
|
|
bool MetaCorePhysicsWorld::SetLinearVelocity(MetaCoreId id, glm::vec3 value) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end()) return false; it->second->Body->setLinearVelocity(Bt(value)); it->second->Body->activate(true); return true; }
|
|
std::optional<glm::vec3> MetaCorePhysicsWorld::GetLinearVelocity(MetaCoreId id) const { const auto it = Impl_->Bodies.find(id); return it == Impl_->Bodies.end() ? std::nullopt : std::optional<glm::vec3>(Glm(it->second->Body->getLinearVelocity())); }
|
|
bool MetaCorePhysicsWorld::WakeUp(MetaCoreId id) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end()) return false; it->second->Body->activate(true); return true; }
|
|
bool MetaCorePhysicsWorld::CharacterMove(MetaCoreId id, glm::vec3 displacement) {
|
|
const auto it = Impl_->Bodies.find(id);
|
|
if (it == Impl_->Bodies.end() || !it->second->Character || !std::isfinite(displacement.x) || !std::isfinite(displacement.y)) return false;
|
|
auto& entry = *it->second;
|
|
const float dt = static_cast<float>(Impl_->Settings.FixedTimeStep);
|
|
glm::vec3 horizontal(displacement.x, displacement.y, 0.0F);
|
|
btVector3 velocity = entry.Body->getLinearVelocity();
|
|
if (glm::dot(horizontal, horizontal) < 1.0e-12F) { velocity.setX(0.0F); velocity.setY(0.0F); entry.Body->setLinearVelocity(velocity); return true; }
|
|
auto* capsule = entry.ChildShapes.empty() ? nullptr : dynamic_cast<btConvexShape*>(entry.ChildShapes.front().get());
|
|
if (capsule == nullptr) return false;
|
|
struct Sweep final : btCollisionWorld::ClosestConvexResultCallback {
|
|
const btCollisionObject* Ignored;
|
|
Sweep(const btVector3& from, const btVector3& to, const btCollisionObject* ignored)
|
|
: ClosestConvexResultCallback(from, to), Ignored(ignored) {}
|
|
bool needsCollision(btBroadphaseProxy* proxy) const override {
|
|
return proxy && proxy->m_clientObject != Ignored && ClosestConvexResultCallback::needsCollision(proxy);
|
|
}
|
|
};
|
|
const auto sweep = [&](const btTransform& from, const btTransform& to) {
|
|
Sweep result(from.getOrigin(), to.getOrigin(), entry.Body.get());
|
|
Impl_->World->convexSweepTest(capsule, from, to, result, entry.CharacterSkinWidth);
|
|
return result;
|
|
};
|
|
btTransform from = entry.Body->getWorldTransform(), to = from; to.setOrigin(from.getOrigin() + Bt(horizontal));
|
|
Sweep hit = sweep(from, to);
|
|
glm::vec3 resolved = horizontal;
|
|
if (hit.hasHit()) {
|
|
const glm::vec3 normal = Glm(hit.m_hitNormalWorld);
|
|
const float walkableZ = std::cos(glm::radians(std::clamp(entry.CharacterMaxSlopeDegrees, 0.0F, 89.0F)));
|
|
if (normal.z >= walkableZ) {
|
|
resolved -= normal * glm::dot(resolved, normal);
|
|
} else {
|
|
bool stepped = false;
|
|
if (entry.CharacterStepHeight > 0.0F) {
|
|
btTransform raised = from; raised.getOrigin() += btVector3(0.0F, 0.0F, entry.CharacterStepHeight);
|
|
const Sweep upHit = sweep(from, raised);
|
|
btTransform raisedTarget = raised; raisedTarget.getOrigin() += Bt(horizontal);
|
|
const Sweep acrossHit = sweep(raised, raisedTarget);
|
|
if (!upHit.hasHit() && !acrossHit.hasHit()) {
|
|
btTransform downTarget = raisedTarget; downTarget.getOrigin() -= btVector3(0.0F, 0.0F, entry.CharacterStepHeight + entry.CharacterSkinWidth * 2.0F);
|
|
const Sweep downHit = sweep(raisedTarget, downTarget);
|
|
if (downHit.hasHit() && downHit.m_hitNormalWorld.z() >= walkableZ) {
|
|
const btVector3 landed = raisedTarget.getOrigin().lerp(downTarget.getOrigin(), downHit.m_closestHitFraction);
|
|
raisedTarget.setOrigin(landed); entry.Body->setWorldTransform(raisedTarget); entry.MotionState->setWorldTransform(raisedTarget); stepped = true;
|
|
}
|
|
}
|
|
}
|
|
if (!stepped) resolved -= normal * glm::dot(resolved, normal);
|
|
else resolved = glm::vec3(0.0F);
|
|
if (auto* pushed = btRigidBody::upcast(const_cast<btCollisionObject*>(hit.m_hitCollisionObject)); pushed && !pushed->isStaticOrKinematicObject()) {
|
|
const glm::vec3 direction = glm::normalize(horizontal);
|
|
pushed->activate(true); pushed->applyCentralImpulse(Bt(direction * entry.CharacterPushForce));
|
|
}
|
|
}
|
|
}
|
|
velocity.setX(resolved.x / dt); velocity.setY(resolved.y / dt);
|
|
entry.Body->setLinearVelocity(velocity); entry.Body->activate(true); return true;
|
|
}
|
|
bool MetaCorePhysicsWorld::CharacterJump(MetaCoreId id, float speed) { if (!IsCharacterGrounded(id)) return false; auto value=GetLinearVelocity(id); if(!value)return false; value->z=speed; return SetLinearVelocity(id,*value); }
|
|
bool MetaCorePhysicsWorld::SetConstraintEnabled(MetaCoreId id, bool enabled) {
|
|
const auto iterator = std::find_if(Impl_->Constraints.begin(), Impl_->Constraints.end(), [id](const auto& value) { return value.Owner == id; });
|
|
if (iterator == Impl_->Constraints.end() || !iterator->Value) return false;
|
|
iterator->Value->setEnabled(enabled); if (enabled) iterator->Broken = false; return true;
|
|
}
|
|
bool MetaCorePhysicsWorld::SetConstraintMotor(MetaCoreId id, bool enabled, float targetVelocity, float maxImpulse) {
|
|
const auto iterator = std::find_if(Impl_->Constraints.begin(), Impl_->Constraints.end(), [id](const auto& value) { return value.Owner == id; });
|
|
if (iterator == Impl_->Constraints.end() || !iterator->Value || !std::isfinite(targetVelocity) || !std::isfinite(maxImpulse) || maxImpulse < 0.0F) return false;
|
|
if (auto* hinge = dynamic_cast<btHingeConstraint*>(iterator->Value.get())) { hinge->enableAngularMotor(enabled, targetVelocity, maxImpulse); return true; }
|
|
if (auto* slider = dynamic_cast<btSliderConstraint*>(iterator->Value.get())) { slider->setPoweredLinMotor(enabled); slider->setTargetLinMotorVelocity(targetVelocity); slider->setMaxLinMotorForce(maxImpulse); return true; }
|
|
if (auto* sixDof = dynamic_cast<btGeneric6DofConstraint*>(iterator->Value.get())) {
|
|
auto* motor = sixDof->getTranslationalLimitMotor(); motor->m_enableMotor[0] = enabled; motor->m_targetVelocity[0] = targetVelocity; motor->m_maxMotorForce[0] = maxImpulse; return true;
|
|
}
|
|
return false;
|
|
}
|
|
bool MetaCorePhysicsWorld::IsCharacterGrounded(MetaCoreId id) const {
|
|
const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || !it->second->Character) return false;
|
|
const btVector3 from = it->second->Body->getWorldTransform().getOrigin();
|
|
const float distance = it->second->CharacterHalfHeight * it->second->Scale.z + it->second->CharacterSkinWidth + 0.08F;
|
|
struct Callback final : btCollisionWorld::ClosestRayResultCallback {
|
|
const btCollisionObject* Ignored;
|
|
Callback(const btVector3& start, const btVector3& end, const btCollisionObject* ignored)
|
|
: ClosestRayResultCallback(start, end), Ignored(ignored) {}
|
|
bool needsCollision(btBroadphaseProxy* proxy) const override {
|
|
return proxy && proxy->m_clientObject != Ignored && ClosestRayResultCallback::needsCollision(proxy);
|
|
}
|
|
} callback(from, from + btVector3(0.0F, 0.0F, -distance), it->second->Body.get());
|
|
Impl_->World->rayTest(callback.m_rayFromWorld, callback.m_rayToWorld, callback);
|
|
const float walkableZ = std::cos(glm::radians(std::clamp(it->second->CharacterMaxSlopeDegrees, 0.0F, 89.0F)));
|
|
return callback.hasHit() && callback.m_hitNormalWorld.z() >= walkableZ;
|
|
}
|
|
|
|
std::vector<MetaCorePhysicsDebugLine> MetaCorePhysicsWorld::BuildDebugLines() const {
|
|
std::vector<MetaCorePhysicsDebugLine> lines;
|
|
struct DebugDrawer final : btIDebugDraw {
|
|
std::vector<MetaCorePhysicsDebugLine>& Lines;int Mode=DBG_DrawWireframe|DBG_DrawConstraints|DBG_DrawConstraintLimits;
|
|
explicit DebugDrawer(std::vector<MetaCorePhysicsDebugLine>& lines):Lines(lines){}
|
|
void drawLine(const btVector3& from,const btVector3& to,const btVector3& color) override{Lines.push_back({Glm(from),Glm(to),Glm(color)});}
|
|
void drawContactPoint(const btVector3& point,const btVector3& normal,btScalar distance,int,const btVector3& color) override{drawLine(point,point+normal*distance,color);}
|
|
void reportErrorWarning(const char*) override{}void draw3dText(const btVector3&,const char*) override{}void setDebugMode(int mode) override{Mode=mode;}int getDebugMode() const override{return Mode;}
|
|
} drawer(lines);
|
|
Impl_->World->setDebugDrawer(&drawer);Impl_->World->debugDrawWorld();Impl_->World->setDebugDrawer(nullptr);
|
|
const auto addBox = [&](glm::vec3 min, glm::vec3 max, glm::vec3 color) {
|
|
const glm::vec3 c[8]{{min.x,min.y,min.z},{max.x,min.y,min.z},{max.x,max.y,min.z},{min.x,max.y,min.z},{min.x,min.y,max.z},{max.x,min.y,max.z},{max.x,max.y,max.z},{min.x,max.y,max.z}};
|
|
constexpr int edges[12][2]{{0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},{0,4},{1,5},{2,6},{3,7}}; for (auto& edge : edges) lines.push_back({c[edge[0]], c[edge[1]], color});
|
|
};
|
|
for (const auto& [id, entry] : Impl_->Bodies) {
|
|
(void)id; btVector3 min, max; entry->Body->getAabb(min, max);
|
|
const glm::vec3 color = entry->Character ? glm::vec3(0.85F,0.3F,1.0F) : entry->Meta.Trigger ? glm::vec3(1.0F,0.7F,0.1F) : entry->Body->isActive() ? glm::vec3(0.2F,1.0F,0.3F) : glm::vec3(0.3F,0.5F,1.0F);
|
|
addBox(Glm(min), Glm(max), color);
|
|
}
|
|
for (const auto& [key, contact] : Impl_->PreviousPairs) {
|
|
(void)key;
|
|
lines.push_back({contact.Point - glm::vec3(0.04F,0.0F,0.0F), contact.Point + glm::vec3(0.04F,0.0F,0.0F), glm::vec3(1.0F,0.2F,0.2F)});
|
|
lines.push_back({contact.Point - glm::vec3(0.0F,0.04F,0.0F), contact.Point + glm::vec3(0.0F,0.04F,0.0F), glm::vec3(1.0F,0.2F,0.2F)});
|
|
lines.push_back({contact.Point, contact.Point + contact.Normal * 0.35F, glm::vec3(1.0F,0.55F,0.15F)});
|
|
}
|
|
for (const auto& constraint : Impl_->Constraints) {
|
|
if (!constraint.Value) continue;
|
|
const glm::vec3 a = Glm(constraint.Value->getRigidBodyA().getWorldTransform().getOrigin());
|
|
const glm::vec3 b = constraint.Target == 0 ? a : Glm(constraint.Value->getRigidBodyB().getWorldTransform().getOrigin());
|
|
lines.push_back({a, b, constraint.Broken ? glm::vec3(1.0F,0.1F,0.1F) : glm::vec3(0.8F,0.35F,1.0F)});
|
|
lines.push_back({a, a + glm::vec3(0.0F,0.0F,0.3F), glm::vec3(0.8F,0.35F,1.0F)});
|
|
}
|
|
lines.insert(lines.end(), Impl_->QueryDebugLines.begin(), Impl_->QueryDebugLines.end());
|
|
return lines;
|
|
}
|
|
const MetaCorePhysicsStatistics& MetaCorePhysicsWorld::GetStatistics() const { return Impl_->Statistics; }
|
|
const std::vector<std::string>& MetaCorePhysicsWorld::GetDiagnostics() const { return Impl_->Diagnostics; }
|
|
const MetaCorePhysicsSettingsDocument& MetaCorePhysicsWorld::GetSettings() const { return Impl_->Settings; }
|
|
|
|
class MetaCoreBulletPhysicsBackend final : public MetaCoreIPhysicsBackend {
|
|
public:
|
|
std::string GetBackendId() const override { return "Bullet3"; }
|
|
std::unique_ptr<MetaCorePhysicsWorld> CreateWorld(MetaCoreScene& scene, const MetaCorePhysicsSettingsDocument& settings,
|
|
MetaCorePhysicsMeshProvider mesh, MetaCorePhysicsMaterialProvider material) override {
|
|
return std::make_unique<MetaCorePhysicsWorld>(scene, settings, std::move(mesh), std::move(material));
|
|
}
|
|
};
|
|
std::unique_ptr<MetaCoreIPhysicsBackend> MetaCoreCreateBulletPhysicsBackend() { return std::make_unique<MetaCoreBulletPhysicsBackend>(); }
|
|
|
|
} // namespace MetaCore
|