Rename ChipmunkPhysics2D and JoltPhysics3D to Physics[2D|3D]
This commit is contained in:
Lynix
463
src/Nazara/Physics3D/Collider3D.cpp
Normal file
463
src/Nazara/Physics3D/Collider3D.cpp
Normal file
@@ -0,0 +1,463 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/Collider3D.hpp>
|
||||
#include <Nazara/Core/PrimitiveList.hpp>
|
||||
#include <Nazara/Physics3D/JoltHelper.hpp>
|
||||
#include <Nazara/Utility/Algorithm.hpp>
|
||||
#include <Nazara/Utility/IndexBuffer.hpp>
|
||||
#include <Nazara/Utility/SoftwareBuffer.hpp>
|
||||
#include <Nazara/Utility/StaticMesh.hpp>
|
||||
#include <Nazara/Utility/VertexBuffer.hpp>
|
||||
#include <NazaraUtils/StackArray.hpp>
|
||||
#include <Jolt/Core/Core.h>
|
||||
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
|
||||
#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
|
||||
#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
|
||||
#include <Jolt/Physics/Collision/Shape/MeshShape.h>
|
||||
#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
|
||||
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
|
||||
#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
|
||||
#include <optional>
|
||||
#include <unordered_map>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
Collider3D::Collider3D() = default;
|
||||
Collider3D::~Collider3D() = default;
|
||||
|
||||
std::shared_ptr<StaticMesh> Collider3D::GenerateDebugMesh() const
|
||||
{
|
||||
std::vector<Vector3f> colliderVertices;
|
||||
std::vector<UInt16> colliderIndices;
|
||||
BuildDebugMesh(colliderVertices, colliderIndices);
|
||||
|
||||
std::shared_ptr<VertexBuffer> colliderVB = std::make_shared<VertexBuffer>(VertexDeclaration::Get(VertexLayout::XYZ), SafeCast<UInt32>(colliderVertices.size()), BufferUsage::Write, SoftwareBufferFactory, colliderVertices.data());
|
||||
std::shared_ptr<IndexBuffer> colliderIB = std::make_shared<IndexBuffer>(IndexType::U16, SafeCast<UInt32>(colliderIndices.size()), BufferUsage::Write, SoftwareBufferFactory, colliderIndices.data());
|
||||
|
||||
std::shared_ptr<StaticMesh> colliderSubMesh = std::make_shared<StaticMesh>(std::move(colliderVB), std::move(colliderIB));
|
||||
colliderSubMesh->GenerateAABB();
|
||||
colliderSubMesh->SetPrimitiveMode(PrimitiveMode::LineList);
|
||||
|
||||
return colliderSubMesh;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collider3D> Collider3D::Build(const PrimitiveList& list)
|
||||
{
|
||||
std::size_t primitiveCount = list.GetSize();
|
||||
if (primitiveCount > 1)
|
||||
{
|
||||
std::vector<CompoundCollider3D::ChildCollider> childColliders(primitiveCount);
|
||||
|
||||
for (unsigned int i = 0; i < primitiveCount; ++i)
|
||||
{
|
||||
const Primitive& primitive = list.GetPrimitive(i);
|
||||
childColliders[i].collider = CreateGeomFromPrimitive(primitive);
|
||||
childColliders[i].offset = primitive.matrix.GetTranslation();
|
||||
childColliders[i].rotation = primitive.matrix.GetRotation();
|
||||
}
|
||||
|
||||
return std::make_shared<CompoundCollider3D>(std::move(childColliders));
|
||||
}
|
||||
else if (primitiveCount > 0)
|
||||
return CreateGeomFromPrimitive(list.GetPrimitive(0));
|
||||
else
|
||||
return nullptr;// std::make_shared<NullCollider3D>(); //< TODO
|
||||
}
|
||||
|
||||
void Collider3D::ResetShapeSettings()
|
||||
{
|
||||
m_shapeSettings.reset();
|
||||
}
|
||||
|
||||
void Collider3D::SetupShapeSettings(std::unique_ptr<JPH::ShapeSettings>&& shapeSettings)
|
||||
{
|
||||
assert(!m_shapeSettings);
|
||||
m_shapeSettings = std::move(shapeSettings);
|
||||
m_shapeSettings->SetEmbedded();
|
||||
|
||||
if (auto result = m_shapeSettings->Create(); result.HasError())
|
||||
throw std::runtime_error(std::string("shape creation failed: ") + result.GetError().c_str());
|
||||
}
|
||||
|
||||
std::shared_ptr<Collider3D> Collider3D::CreateGeomFromPrimitive(const Primitive& primitive)
|
||||
{
|
||||
switch (primitive.type)
|
||||
{
|
||||
case PrimitiveType::Box:
|
||||
return std::make_shared<BoxCollider3D>(primitive.box.lengths);
|
||||
|
||||
case PrimitiveType::Cone:
|
||||
return nullptr; //< TODO
|
||||
//return std::make_shared<ConeCollider3D>(primitive.cone.length, primitive.cone.radius);
|
||||
|
||||
case PrimitiveType::Plane:
|
||||
return std::make_shared<BoxCollider3D>(Vector3f(primitive.plane.size.x, 0.01f, primitive.plane.size.y));
|
||||
///TODO: PlaneGeom?
|
||||
|
||||
case PrimitiveType::Sphere:
|
||||
return std::make_shared<SphereCollider3D>(primitive.sphere.size);
|
||||
}
|
||||
|
||||
NazaraErrorFmt("Primitive type not handled ({0:#x})", UnderlyingCast(primitive.type));
|
||||
return std::shared_ptr<Collider3D>();
|
||||
}
|
||||
|
||||
/********************************** BoxCollider3D **********************************/
|
||||
|
||||
BoxCollider3D::BoxCollider3D(const Vector3f& lengths, float convexRadius)
|
||||
{
|
||||
SetupShapeSettings(std::make_unique<JPH::BoxShapeSettings>(ToJolt(lengths * 0.5f), convexRadius));
|
||||
}
|
||||
|
||||
void BoxCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
auto InsertVertex = [&](float x, float y, float z) -> UInt16
|
||||
{
|
||||
UInt16 index = SafeCast<UInt16>(vertices.size());
|
||||
vertices.push_back(offsetMatrix * Vector3f(x, y, z));
|
||||
return index;
|
||||
};
|
||||
|
||||
Vector3f halfLengths = FromJolt(GetShapeSettingsAs<JPH::BoxShapeSettings>()->mHalfExtent);
|
||||
|
||||
UInt16 v0 = InsertVertex(-halfLengths.x, -halfLengths.y, -halfLengths.z);
|
||||
UInt16 v1 = InsertVertex(halfLengths.x, -halfLengths.y, -halfLengths.z);
|
||||
UInt16 v2 = InsertVertex(halfLengths.x, -halfLengths.y, halfLengths.z);
|
||||
UInt16 v3 = InsertVertex(-halfLengths.x, -halfLengths.y, halfLengths.z);
|
||||
|
||||
UInt16 v4 = InsertVertex(-halfLengths.x, halfLengths.y, -halfLengths.z);
|
||||
UInt16 v5 = InsertVertex(halfLengths.x, halfLengths.y, -halfLengths.z);
|
||||
UInt16 v6 = InsertVertex(halfLengths.x, halfLengths.y, halfLengths.z);
|
||||
UInt16 v7 = InsertVertex(-halfLengths.x, halfLengths.y, halfLengths.z);
|
||||
|
||||
auto InsertEdge = [&](UInt16 from, UInt16 to)
|
||||
{
|
||||
indices.push_back(from);
|
||||
indices.push_back(to);
|
||||
};
|
||||
InsertEdge(v0, v1);
|
||||
InsertEdge(v1, v2);
|
||||
InsertEdge(v2, v3);
|
||||
InsertEdge(v3, v0);
|
||||
|
||||
InsertEdge(v4, v5);
|
||||
InsertEdge(v5, v6);
|
||||
InsertEdge(v6, v7);
|
||||
InsertEdge(v7, v4);
|
||||
|
||||
InsertEdge(v0, v4);
|
||||
InsertEdge(v1, v5);
|
||||
InsertEdge(v2, v6);
|
||||
InsertEdge(v3, v7);
|
||||
}
|
||||
|
||||
Vector3f BoxCollider3D::GetLengths() const
|
||||
{
|
||||
return FromJolt(GetShapeSettingsAs<JPH::BoxShapeSettings>()->mHalfExtent) * 2.f;
|
||||
}
|
||||
|
||||
ColliderType3D BoxCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::Box;
|
||||
}
|
||||
|
||||
/********************************** CapsuleCollider3D **********************************/
|
||||
|
||||
CapsuleCollider3D::CapsuleCollider3D(float height, float radius)
|
||||
{
|
||||
SetupShapeSettings(std::make_unique<JPH::CapsuleShapeSettings>(height * 0.5f, radius));
|
||||
}
|
||||
|
||||
void CapsuleCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
constexpr unsigned int cornerStepCount = 10;
|
||||
constexpr unsigned int sliceCount = 4;
|
||||
|
||||
const JPH::CapsuleShapeSettings* capsuleSettings = GetShapeSettingsAs<JPH::CapsuleShapeSettings>();
|
||||
float radius = capsuleSettings->mRadius;
|
||||
float halfHeight = capsuleSettings->mHalfHeightOfCylinder;
|
||||
|
||||
std::optional<UInt16> firstVertex;
|
||||
std::optional<UInt16> lastVertex;
|
||||
|
||||
for (unsigned int slice = 0; slice < sliceCount; ++slice)
|
||||
{
|
||||
Quaternionf rot(RadianAnglef(2.f * Pi<float> * slice / sliceCount), Nz::Vector3f::Down());
|
||||
|
||||
Vector3f top(0.f, halfHeight, 0.f);
|
||||
for (unsigned int i = 0; i < cornerStepCount; ++i)
|
||||
{
|
||||
auto [sin, cos] = RadianAnglef(0.5f * Pi<float> * i / cornerStepCount).GetSinCos();
|
||||
|
||||
UInt16 index;
|
||||
if (firstVertex && i == 0)
|
||||
index = *firstVertex;
|
||||
else
|
||||
{
|
||||
index = SafeCast<UInt16>(vertices.size());
|
||||
vertices.push_back(offsetMatrix * (top + rot * (radius * Vector3f(sin, cos, 0.f))));
|
||||
if (i == 0)
|
||||
firstVertex = index;
|
||||
}
|
||||
|
||||
if (i > 1)
|
||||
{
|
||||
indices.push_back(static_cast<UInt16>(index - 1));
|
||||
indices.push_back(index);
|
||||
}
|
||||
else if (i == 1)
|
||||
{
|
||||
indices.push_back(*firstVertex);
|
||||
indices.push_back(index);
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f bottom(0.f, -halfHeight, 0.f);
|
||||
for (unsigned int i = 0; i < cornerStepCount; ++i)
|
||||
{
|
||||
auto [sin, cos] = RadianAnglef(0.5f * Pi<float> * i / cornerStepCount).GetSinCos();
|
||||
|
||||
UInt16 index;
|
||||
if (lastVertex && i == sliceCount - 1)
|
||||
index = *lastVertex;
|
||||
else
|
||||
{
|
||||
index = SafeCast<UInt16>(vertices.size());
|
||||
vertices.push_back(offsetMatrix * (bottom - rot * (radius * Vector3f(-cos, sin, 0.f))));
|
||||
if (i == sliceCount - 1)
|
||||
lastVertex = index;
|
||||
}
|
||||
|
||||
indices.push_back(static_cast<UInt16>(index - 1));
|
||||
indices.push_back(index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float CapsuleCollider3D::GetHeight() const
|
||||
{
|
||||
return GetShapeSettingsAs<JPH::CapsuleShapeSettings>()->mHalfHeightOfCylinder * 2.0f;
|
||||
}
|
||||
|
||||
float CapsuleCollider3D::GetRadius() const
|
||||
{
|
||||
return GetShapeSettingsAs<JPH::CapsuleShapeSettings>()->mRadius;
|
||||
}
|
||||
|
||||
ColliderType3D CapsuleCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::Capsule;
|
||||
}
|
||||
|
||||
/******************************* CompoundCollider3D ********************************/
|
||||
|
||||
CompoundCollider3D::CompoundCollider3D(std::vector<ChildCollider> childs) :
|
||||
m_childs(std::move(childs))
|
||||
{
|
||||
auto shapeSettings = std::make_unique<JPH::StaticCompoundShapeSettings>();
|
||||
for (const auto& child : m_childs)
|
||||
shapeSettings->AddShape(ToJolt(child.offset), ToJolt(child.rotation), child.collider->GetShapeSettings());
|
||||
|
||||
SetupShapeSettings(std::move(shapeSettings));
|
||||
}
|
||||
|
||||
CompoundCollider3D::~CompoundCollider3D()
|
||||
{
|
||||
// We have to destroy shape settings first as it carries references on the inner colliders
|
||||
ResetShapeSettings();
|
||||
}
|
||||
|
||||
void CompoundCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
for (const auto& child : m_childs)
|
||||
child.collider->BuildDebugMesh(vertices, indices, offsetMatrix * Matrix4f::Transform(child.offset, child.rotation));
|
||||
}
|
||||
|
||||
auto CompoundCollider3D::GetGeoms() const -> const std::vector<ChildCollider>&
|
||||
{
|
||||
return m_childs;
|
||||
}
|
||||
|
||||
ColliderType3D CompoundCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::Compound;
|
||||
}
|
||||
|
||||
/******************************** ConvexHullCollider3D *********************************/
|
||||
|
||||
ConvexHullCollider3D::ConvexHullCollider3D(SparsePtr<const Vector3f> vertices, std::size_t vertexCount, float hullTolerance, float convexRadius, float maxErrorConvexRadius)
|
||||
{
|
||||
std::unique_ptr<JPH::ConvexHullShapeSettings> settings = std::make_unique<JPH::ConvexHullShapeSettings>();
|
||||
settings->mHullTolerance = hullTolerance;
|
||||
settings->mMaxConvexRadius = convexRadius;
|
||||
settings->mMaxErrorConvexRadius = maxErrorConvexRadius;
|
||||
|
||||
settings->mPoints.resize(vertexCount);
|
||||
for (std::size_t i = 0; i < vertexCount; ++i)
|
||||
settings->mPoints[i] = ToJolt(vertices[i]);
|
||||
|
||||
SetupShapeSettings(std::move(settings));
|
||||
}
|
||||
|
||||
void ConvexHullCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
const JPH::ConvexHullShapeSettings* settings = GetShapeSettingsAs<JPH::ConvexHullShapeSettings>();
|
||||
const JPH::ConvexHullShape* shape = SafeCast<const JPH::ConvexHullShape*>(settings->Create().Get().GetPtr());
|
||||
|
||||
std::unordered_map<unsigned int, UInt16> vertexCache;
|
||||
auto InsertVertex = [&](unsigned int vertexIndex) -> UInt16
|
||||
{
|
||||
auto it = vertexCache.find(vertexIndex);
|
||||
if (it != vertexCache.end())
|
||||
return it->second;
|
||||
|
||||
UInt16 index = SafeCast<UInt16>(vertices.size());
|
||||
|
||||
vertices.push_back(offsetMatrix * FromJolt(shape->GetPoint(vertexIndex)));
|
||||
vertexCache.emplace(vertexIndex, index);
|
||||
|
||||
return index;
|
||||
};
|
||||
|
||||
unsigned int faceCount = shape->GetNumFaces();
|
||||
unsigned int maxVerticesInFace = 0;
|
||||
for (unsigned int i = 0; i < faceCount; ++i)
|
||||
maxVerticesInFace = std::max(maxVerticesInFace, shape->GetNumVerticesInFace(i));
|
||||
|
||||
StackArray<unsigned int> faceVertices = NazaraStackArrayNoInit(unsigned int, maxVerticesInFace);
|
||||
for (unsigned int i = 0; i < faceCount; ++i)
|
||||
{
|
||||
unsigned int vertexCount = shape->GetFaceVertices(i, maxVerticesInFace, faceVertices.data());
|
||||
if NAZARA_LIKELY(vertexCount >= 2)
|
||||
{
|
||||
for (unsigned int j = 1; j < vertexCount; ++j)
|
||||
{
|
||||
indices.push_back(InsertVertex(faceVertices[j - 1]));
|
||||
indices.push_back(InsertVertex(faceVertices[j]));
|
||||
}
|
||||
|
||||
if NAZARA_LIKELY(vertexCount > 2)
|
||||
{
|
||||
indices.push_back(InsertVertex(faceVertices[vertexCount - 1]));
|
||||
indices.push_back(InsertVertex(faceVertices[0]));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ColliderType3D ConvexHullCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::Convex;
|
||||
}
|
||||
|
||||
/******************************** MeshCollider3D *********************************/
|
||||
|
||||
MeshCollider3D::MeshCollider3D(SparsePtr<const Vector3f> vertices, std::size_t vertexCount, SparsePtr<const UInt16> indices, std::size_t indexCount)
|
||||
{
|
||||
std::unique_ptr<JPH::MeshShapeSettings> settings = std::make_unique<JPH::MeshShapeSettings>();
|
||||
settings->mTriangleVertices.resize(vertexCount);
|
||||
for (std::size_t i = 0; i < vertexCount; ++i)
|
||||
{
|
||||
settings->mTriangleVertices[i].x = vertices[i].x;
|
||||
settings->mTriangleVertices[i].y = vertices[i].y;
|
||||
settings->mTriangleVertices[i].z = vertices[i].z;
|
||||
}
|
||||
|
||||
std::size_t triangleCount = indexCount / 3;
|
||||
settings->mIndexedTriangles.resize(triangleCount);
|
||||
for (std::size_t i = 0; i < triangleCount; ++i)
|
||||
{
|
||||
settings->mIndexedTriangles[i].mIdx[0] = indices[i * 3 + 0];
|
||||
settings->mIndexedTriangles[i].mIdx[1] = indices[i * 3 + 1];
|
||||
settings->mIndexedTriangles[i].mIdx[2] = indices[i * 3 + 2];
|
||||
}
|
||||
|
||||
settings->Sanitize();
|
||||
|
||||
SetupShapeSettings(std::move(settings));
|
||||
}
|
||||
|
||||
MeshCollider3D::MeshCollider3D(SparsePtr<const Vector3f> vertices, std::size_t vertexCount, SparsePtr<const UInt32> indices, std::size_t indexCount)
|
||||
{
|
||||
std::unique_ptr<JPH::MeshShapeSettings> settings = std::make_unique<JPH::MeshShapeSettings>();
|
||||
settings->mTriangleVertices.resize(vertexCount);
|
||||
for (std::size_t i = 0; i < vertexCount; ++i)
|
||||
{
|
||||
settings->mTriangleVertices[i].x = vertices[i].x;
|
||||
settings->mTriangleVertices[i].y = vertices[i].y;
|
||||
settings->mTriangleVertices[i].z = vertices[i].z;
|
||||
}
|
||||
|
||||
std::size_t triangleCount = indexCount / 3;
|
||||
settings->mIndexedTriangles.resize(triangleCount);
|
||||
for (std::size_t i = 0; i < triangleCount; ++i)
|
||||
{
|
||||
settings->mIndexedTriangles[i].mIdx[0] = indices[i * 3 + 0];
|
||||
settings->mIndexedTriangles[i].mIdx[1] = indices[i * 3 + 1];
|
||||
settings->mIndexedTriangles[i].mIdx[2] = indices[i * 3 + 2];
|
||||
}
|
||||
|
||||
settings->Sanitize();
|
||||
|
||||
SetupShapeSettings(std::move(settings));
|
||||
}
|
||||
|
||||
void MeshCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
}
|
||||
|
||||
ColliderType3D MeshCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::Mesh;
|
||||
}
|
||||
|
||||
/******************************** SphereCollider3D *********************************/
|
||||
|
||||
SphereCollider3D::SphereCollider3D(float radius)
|
||||
{
|
||||
SetupShapeSettings(std::make_unique<JPH::SphereShapeSettings>(radius));
|
||||
}
|
||||
|
||||
void SphereCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
}
|
||||
|
||||
float SphereCollider3D::GetRadius() const
|
||||
{
|
||||
return GetShapeSettingsAs<JPH::SphereShapeSettings>()->mRadius;
|
||||
}
|
||||
|
||||
ColliderType3D SphereCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::Sphere;
|
||||
}
|
||||
|
||||
/******************************** TranslatedRotatedCollider3D *********************************/
|
||||
|
||||
TranslatedRotatedCollider3D::TranslatedRotatedCollider3D(std::shared_ptr<Collider3D> collider, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
m_collider(std::move(collider))
|
||||
{
|
||||
SetupShapeSettings(std::make_unique<JPH::RotatedTranslatedShapeSettings>(ToJolt(translation), ToJolt(rotation), m_collider->GetShapeSettings()));
|
||||
}
|
||||
|
||||
TranslatedRotatedCollider3D::~TranslatedRotatedCollider3D()
|
||||
{
|
||||
// We have to destroy shape settings first as it carries references on the inner collider
|
||||
ResetShapeSettings();
|
||||
}
|
||||
|
||||
void TranslatedRotatedCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
||||
{
|
||||
const JPH::RotatedTranslatedShapeSettings* settings = GetShapeSettingsAs<JPH::RotatedTranslatedShapeSettings>();
|
||||
|
||||
m_collider->BuildDebugMesh(vertices, indices, offsetMatrix * Matrix4f::Transform(FromJolt(settings->mPosition), FromJolt(settings->mRotation)));
|
||||
}
|
||||
|
||||
ColliderType3D TranslatedRotatedCollider3D::GetType() const
|
||||
{
|
||||
return ColliderType3D::TranslatedRotatedDecoration;
|
||||
}
|
||||
}
|
||||
30
src/Nazara/Physics3D/JoltHelper.hpp
Normal file
30
src/Nazara/Physics3D/JoltHelper.hpp
Normal file
@@ -0,0 +1,30 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifndef NAZARA_PHYSICS3D_JOLTHELPER_HPP
|
||||
#define NAZARA_PHYSICS3D_JOLTHELPER_HPP
|
||||
|
||||
#include <NazaraUtils/Prerequisites.hpp>
|
||||
#include <Nazara/Math/Matrix4.hpp>
|
||||
#include <Nazara/Math/Quaternion.hpp>
|
||||
#include <Nazara/Math/Vector3.hpp>
|
||||
#include <Jolt/Jolt.h>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
inline Quaternionf FromJolt(const JPH::Quat& quat);
|
||||
inline Matrix4f FromJolt(const JPH::Mat44& matrix);
|
||||
inline Vector3f FromJolt(const JPH::Vec3& vec);
|
||||
|
||||
inline JPH::Mat44 ToJolt(const Matrix4f& transformMatrix);
|
||||
inline JPH::Quat ToJolt(const Quaternionf& quaternion);
|
||||
inline JPH::Vec3 ToJolt(const Vector3f& vec);
|
||||
inline JPH::Vec4 ToJolt(const Vector4f& vec);
|
||||
}
|
||||
|
||||
#include <Nazara/Physics3D/JoltHelper.inl>
|
||||
|
||||
#endif // NAZARA_PHYSICS3D_JOLTHELPER_HPP
|
||||
60
src/Nazara/Physics3D/JoltHelper.inl
Normal file
60
src/Nazara/Physics3D/JoltHelper.inl
Normal file
@@ -0,0 +1,60 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
Quaternionf FromJolt(const JPH::Quat& quat)
|
||||
{
|
||||
return Quaternionf(quat.GetW(), quat.GetX(), quat.GetY(), quat.GetZ());
|
||||
}
|
||||
|
||||
Matrix4f FromJolt(const JPH::Mat44& transform)
|
||||
{
|
||||
const JPH::Vec4& row1 = transform.GetColumn4(0);
|
||||
const JPH::Vec4& row2 = transform.GetColumn4(1);
|
||||
const JPH::Vec4& row3 = transform.GetColumn4(2);
|
||||
const JPH::Vec4& row4 = transform.GetColumn4(3);
|
||||
|
||||
return Matrix4f(
|
||||
row1.GetX(), row1.GetY(), row1.GetZ(), row1.GetW(),
|
||||
row2.GetX(), row2.GetY(), row2.GetZ(), row2.GetW(),
|
||||
row3.GetX(), row3.GetY(), row3.GetZ(), row3.GetW(),
|
||||
row4.GetX(), row4.GetY(), row4.GetZ(), row4.GetW());
|
||||
}
|
||||
|
||||
inline Vector3f FromJolt(const JPH::Vec3& vec)
|
||||
{
|
||||
return Vector3f(vec.GetX(), vec.GetY(), vec.GetZ());
|
||||
}
|
||||
|
||||
inline JPH::Mat44 ToJolt(const Matrix4f& transformMatrix)
|
||||
{
|
||||
JPH::Mat44 transform;
|
||||
transform.SetColumn4(0, JPH::Vec4{ transformMatrix.m11, transformMatrix.m12, transformMatrix.m13, transformMatrix.m14 });
|
||||
transform.SetColumn4(1, JPH::Vec4{ transformMatrix.m21, transformMatrix.m22, transformMatrix.m23, transformMatrix.m24 });
|
||||
transform.SetColumn4(2, JPH::Vec4{ transformMatrix.m31, transformMatrix.m32, transformMatrix.m33, transformMatrix.m34 });
|
||||
transform.SetColumn4(3, JPH::Vec4{ transformMatrix.m41, transformMatrix.m42, transformMatrix.m43, transformMatrix.m44 });
|
||||
|
||||
return transform;
|
||||
}
|
||||
|
||||
inline JPH::Quat ToJolt(const Quaternionf& quaternion)
|
||||
{
|
||||
return JPH::Quat(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
|
||||
}
|
||||
|
||||
inline JPH::Vec3 ToJolt(const Vector3f& vec)
|
||||
{
|
||||
return JPH::Vec3(vec.x, vec.y, vec.z);
|
||||
}
|
||||
|
||||
inline JPH::Vec4 ToJolt(const Vector4f& vec)
|
||||
{
|
||||
return JPH::Vec4(vec.x, vec.y, vec.z, vec.w);
|
||||
}
|
||||
}
|
||||
|
||||
#include <Nazara/Physics3D/DebugOff.hpp>
|
||||
221
src/Nazara/Physics3D/PhysCharacter3D.cpp
Normal file
221
src/Nazara/Physics3D/PhysCharacter3D.cpp
Normal file
@@ -0,0 +1,221 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/PhysCharacter3D.hpp>
|
||||
#include <Nazara/Physics3D/Collider3D.hpp>
|
||||
#include <Nazara/Physics3D/JoltHelper.hpp>
|
||||
#include <Nazara/Physics3D/PhysWorld3D.hpp>
|
||||
#include <Jolt/Jolt.h>
|
||||
#include <Jolt/Physics/PhysicsSystem.h>
|
||||
#include <Jolt/Physics/Character/Character.h>
|
||||
#include <Jolt/Physics/Collision/ObjectLayer.h>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
PhysCharacter3D::PhysCharacter3D() = default;
|
||||
|
||||
PhysCharacter3D::PhysCharacter3D(PhysWorld3D& physWorld, const Settings& settings)
|
||||
{
|
||||
Create(physWorld, settings);
|
||||
}
|
||||
|
||||
PhysCharacter3D::PhysCharacter3D(PhysCharacter3D&& character) noexcept :
|
||||
m_impl(std::move(character.m_impl)),
|
||||
m_collider(std::move(character.m_collider)),
|
||||
m_character(std::move(character.m_character)),
|
||||
m_world(std::move(character.m_world)),
|
||||
m_bodyIndex(character.m_bodyIndex)
|
||||
{
|
||||
character.m_bodyIndex = std::numeric_limits<UInt32>::max();
|
||||
|
||||
if (m_character)
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetUserData(m_character->GetBodyID(), SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
}
|
||||
|
||||
if (m_world)
|
||||
{
|
||||
m_world->UnregisterStepListener(&character);
|
||||
m_world->RegisterStepListener(this);
|
||||
}
|
||||
}
|
||||
|
||||
PhysCharacter3D::~PhysCharacter3D()
|
||||
{
|
||||
Destroy();
|
||||
}
|
||||
|
||||
void PhysCharacter3D::EnableSleeping(bool enable)
|
||||
{
|
||||
const JPH::BodyLockInterfaceNoLock& bodyInterface = m_world->GetPhysicsSystem()->GetBodyLockInterfaceNoLock();
|
||||
JPH::BodyLockWrite bodyLock(bodyInterface, m_character->GetBodyID());
|
||||
if (!bodyLock.Succeeded())
|
||||
return;
|
||||
|
||||
bodyLock.GetBody().SetAllowSleeping(enable);
|
||||
}
|
||||
|
||||
UInt32 PhysCharacter3D::GetBodyIndex() const
|
||||
{
|
||||
return m_bodyIndex;
|
||||
}
|
||||
|
||||
Vector3f PhysCharacter3D::GetLinearVelocity() const
|
||||
{
|
||||
return FromJolt(m_character->GetLinearVelocity(false));
|
||||
}
|
||||
|
||||
Quaternionf PhysCharacter3D::GetRotation() const
|
||||
{
|
||||
return FromJolt(m_character->GetRotation(false));
|
||||
}
|
||||
|
||||
Vector3f PhysCharacter3D::GetPosition() const
|
||||
{
|
||||
return FromJolt(m_character->GetPosition(false));
|
||||
}
|
||||
|
||||
std::pair<Vector3f, Quaternionf> PhysCharacter3D::GetPositionAndRotation() const
|
||||
{
|
||||
JPH::Vec3 position;
|
||||
JPH::Quat rotation;
|
||||
m_character->GetPositionAndRotation(position, rotation, false);
|
||||
|
||||
return { FromJolt(position), FromJolt(rotation) };
|
||||
}
|
||||
|
||||
Vector3f PhysCharacter3D::GetUp() const
|
||||
{
|
||||
return FromJolt(m_character->GetUp());
|
||||
}
|
||||
|
||||
bool PhysCharacter3D::IsOnGround() const
|
||||
{
|
||||
return m_character->GetGroundState() == JPH::Character::EGroundState::OnGround;
|
||||
}
|
||||
|
||||
void PhysCharacter3D::SetFriction(float friction)
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetFriction(m_character->GetBodyID(), friction);
|
||||
}
|
||||
|
||||
void PhysCharacter3D::SetLinearVelocity(const Vector3f& linearVel)
|
||||
{
|
||||
m_character->SetLinearVelocity(ToJolt(linearVel), false);
|
||||
}
|
||||
|
||||
void PhysCharacter3D::SetRotation(const Quaternionf& rotation)
|
||||
{
|
||||
m_character->SetRotation(ToJolt(rotation), JPH::EActivation::Activate, false);
|
||||
}
|
||||
|
||||
void PhysCharacter3D::SetUp(const Vector3f& up)
|
||||
{
|
||||
m_character->SetUp(ToJolt(up));
|
||||
}
|
||||
|
||||
void PhysCharacter3D::TeleportTo(const Vector3f& position, const Quaternionf& rotation)
|
||||
{
|
||||
m_character->SetPositionAndRotation(ToJolt(position), ToJolt(rotation), JPH::EActivation::Activate, false);
|
||||
}
|
||||
|
||||
void PhysCharacter3D::WakeUp()
|
||||
{
|
||||
m_character->Activate(false);
|
||||
}
|
||||
|
||||
PhysCharacter3D& PhysCharacter3D::operator=(PhysCharacter3D&& character) noexcept
|
||||
{
|
||||
Destroy();
|
||||
|
||||
m_impl = std::move(character.m_impl);
|
||||
m_collider = std::move(character.m_collider);
|
||||
m_character = std::move(character.m_character);
|
||||
m_bodyIndex = character.m_bodyIndex;
|
||||
m_world = std::move(character.m_world);
|
||||
|
||||
if (m_world)
|
||||
{
|
||||
m_world->UnregisterStepListener(&character);
|
||||
m_world->RegisterStepListener(this);
|
||||
}
|
||||
|
||||
character.m_bodyIndex = std::numeric_limits<UInt32>::max();
|
||||
|
||||
if (m_character)
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetUserData(m_character->GetBodyID(), SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void PhysCharacter3D::Create(PhysWorld3D& physWorld, const Settings& settings)
|
||||
{
|
||||
m_collider = settings.collider;
|
||||
m_impl = physWorld.GetDefaultCharacterImpl();
|
||||
m_world = &physWorld;
|
||||
|
||||
auto shapeResult = m_collider->GetShapeSettings()->Create();
|
||||
if (!shapeResult.IsValid())
|
||||
throw std::runtime_error("invalid shape");
|
||||
|
||||
JPH::CharacterSettings characterSettings;
|
||||
characterSettings.mShape = shapeResult.Get();
|
||||
characterSettings.mLayer = 1;
|
||||
|
||||
m_character = std::make_unique<JPH::Character>(&characterSettings, ToJolt(settings.position), ToJolt(settings.rotation), 0, m_world->GetPhysicsSystem());
|
||||
m_character->AddToPhysicsSystem();
|
||||
|
||||
m_bodyIndex = m_character->GetBodyID().GetIndex();
|
||||
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetUserData(m_character->GetBodyID(), SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
|
||||
m_world->RegisterStepListener(this);
|
||||
}
|
||||
|
||||
void PhysCharacter3D::Destroy()
|
||||
{
|
||||
if (m_character)
|
||||
{
|
||||
m_character->RemoveFromPhysicsSystem();
|
||||
m_character = nullptr;
|
||||
}
|
||||
|
||||
if (m_world)
|
||||
{
|
||||
m_world->UnregisterStepListener(this);
|
||||
m_world = nullptr;
|
||||
}
|
||||
|
||||
m_collider.reset();
|
||||
}
|
||||
|
||||
void PhysCharacter3D::PostSimulate(float elapsedTime)
|
||||
{
|
||||
m_character->PostSimulation(0.05f);
|
||||
m_impl->PostSimulate(*this, elapsedTime);
|
||||
}
|
||||
|
||||
void PhysCharacter3D::PreSimulate(float elapsedTime)
|
||||
{
|
||||
m_impl->PreSimulate(*this, elapsedTime);
|
||||
}
|
||||
|
||||
|
||||
PhysCharacter3DImpl::~PhysCharacter3DImpl() = default;
|
||||
|
||||
void PhysCharacter3DImpl::PostSimulate(PhysCharacter3D& /*character*/, float /*elapsedTime*/)
|
||||
{
|
||||
}
|
||||
|
||||
void PhysCharacter3DImpl::PreSimulate(PhysCharacter3D& /*character*/, float /*elapsedTime*/)
|
||||
{
|
||||
}
|
||||
}
|
||||
242
src/Nazara/Physics3D/PhysConstraint3D.cpp
Normal file
242
src/Nazara/Physics3D/PhysConstraint3D.cpp
Normal file
@@ -0,0 +1,242 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/PhysConstraint3D.hpp>
|
||||
#include <Nazara/Physics3D/JoltHelper.hpp>
|
||||
#include <Jolt/Jolt.h>
|
||||
#include <Jolt/Physics/PhysicsSystem.h>
|
||||
#include <Jolt/Physics/Constraints/DistanceConstraint.h>
|
||||
#include <Jolt/Physics/Constraints/PointConstraint.h>
|
||||
#include <cassert>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
PhysConstraint3D::PhysConstraint3D() = default;
|
||||
|
||||
PhysConstraint3D::PhysConstraint3D(PhysConstraint3D&& constraint) noexcept :
|
||||
m_constraint(std::move(constraint.m_constraint))
|
||||
{
|
||||
if (m_constraint)
|
||||
m_constraint->SetUserData(SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
}
|
||||
|
||||
PhysConstraint3D::~PhysConstraint3D()
|
||||
{
|
||||
Destroy();
|
||||
}
|
||||
|
||||
RigidBody3D& PhysConstraint3D::GetBodyA()
|
||||
{
|
||||
return *BitCast<RigidBody3D*>(static_cast<std::uintptr_t>(m_constraint->GetBody1()->GetUserData()));
|
||||
}
|
||||
|
||||
const RigidBody3D& PhysConstraint3D::GetBodyA() const
|
||||
{
|
||||
return *BitCast<RigidBody3D*>(static_cast<std::uintptr_t>(m_constraint->GetBody1()->GetUserData()));
|
||||
}
|
||||
|
||||
RigidBody3D& PhysConstraint3D::GetBodyB()
|
||||
{
|
||||
NazaraAssert(!IsSingleBody(), "constraint is not attached to a second body");
|
||||
return *BitCast<RigidBody3D*>(static_cast<std::uintptr_t>(m_constraint->GetBody2()->GetUserData()));
|
||||
}
|
||||
|
||||
const RigidBody3D& PhysConstraint3D::GetBodyB() const
|
||||
{
|
||||
NazaraAssert(!IsSingleBody(), "constraint is not attached to a second body");
|
||||
return *BitCast<RigidBody3D*>(static_cast<std::uintptr_t>(m_constraint->GetBody2()->GetUserData()));
|
||||
}
|
||||
|
||||
PhysWorld3D& PhysConstraint3D::GetWorld()
|
||||
{
|
||||
return GetBodyA().GetWorld();
|
||||
}
|
||||
|
||||
const PhysWorld3D& PhysConstraint3D::GetWorld() const
|
||||
{
|
||||
return GetBodyA().GetWorld();
|
||||
}
|
||||
|
||||
bool PhysConstraint3D::IsSingleBody() const
|
||||
{
|
||||
return m_constraint->GetBody2() == &JPH::Body::sFixedToWorld;
|
||||
}
|
||||
|
||||
PhysConstraint3D& PhysConstraint3D::operator=(PhysConstraint3D&& constraint) noexcept
|
||||
{
|
||||
Destroy();
|
||||
|
||||
m_constraint = std::move(constraint.m_constraint);
|
||||
|
||||
if (m_constraint)
|
||||
m_constraint->SetUserData(SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void PhysConstraint3D::Destroy()
|
||||
{
|
||||
if (m_constraint)
|
||||
{
|
||||
JPH::PhysicsSystem* physicsSystem = GetWorld().GetPhysicsSystem();
|
||||
physicsSystem->RemoveConstraint(m_constraint.get());
|
||||
|
||||
m_constraint.reset();
|
||||
}
|
||||
}
|
||||
|
||||
void PhysConstraint3D::SetupConstraint(std::unique_ptr<JPH::TwoBodyConstraint> constraint)
|
||||
{
|
||||
assert(!m_constraint);
|
||||
m_constraint = std::move(constraint);
|
||||
m_constraint->SetEmbedded();
|
||||
m_constraint->SetUserData(SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
|
||||
JPH::PhysicsSystem* physicsSystem = GetWorld().GetPhysicsSystem();
|
||||
physicsSystem->AddConstraint(m_constraint.get());
|
||||
}
|
||||
|
||||
|
||||
PhysDistanceConstraint3D::PhysDistanceConstraint3D(RigidBody3D& first, const Vector3f& pivot, float maxDist, float minDist)
|
||||
{
|
||||
JPH::DistanceConstraintSettings settings;
|
||||
settings.mPoint1 = ToJolt(pivot);
|
||||
settings.mPoint2 = ToJolt(pivot);
|
||||
settings.mSpace = JPH::EConstraintSpace::WorldSpace;
|
||||
settings.mMaxDistance = maxDist;
|
||||
settings.mMinDistance = minDist;
|
||||
|
||||
SetupConstraint(std::make_unique<JPH::DistanceConstraint>(*first.GetBody(), JPH::Body::sFixedToWorld, settings));
|
||||
}
|
||||
|
||||
PhysDistanceConstraint3D::PhysDistanceConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& pivot, float maxDist, float minDist)
|
||||
{
|
||||
JPH::DistanceConstraintSettings settings;
|
||||
settings.mPoint1 = ToJolt(pivot);
|
||||
settings.mPoint2 = ToJolt(pivot);
|
||||
settings.mSpace = JPH::EConstraintSpace::WorldSpace;
|
||||
settings.mMaxDistance = maxDist;
|
||||
settings.mMinDistance = minDist;
|
||||
|
||||
SetupConstraint(std::make_unique<JPH::DistanceConstraint>(*first.GetBody(), *second.GetBody(), settings));
|
||||
}
|
||||
|
||||
PhysDistanceConstraint3D::PhysDistanceConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& firstAnchor, const Vector3f& secondAnchor, float maxDist, float minDist)
|
||||
{
|
||||
JPH::DistanceConstraintSettings settings;
|
||||
settings.mPoint1 = ToJolt(firstAnchor);
|
||||
settings.mPoint2 = ToJolt(secondAnchor);
|
||||
settings.mSpace = JPH::EConstraintSpace::WorldSpace;
|
||||
settings.mMaxDistance = maxDist;
|
||||
settings.mMinDistance = minDist;
|
||||
|
||||
SetupConstraint(std::make_unique<JPH::DistanceConstraint>(*first.GetBody(), *second.GetBody(), settings));
|
||||
}
|
||||
|
||||
float PhysDistanceConstraint3D::GetDamping() const
|
||||
{
|
||||
return GetConstraint<JPH::DistanceConstraint>()->GetLimitsSpringSettings().mDamping;
|
||||
}
|
||||
|
||||
float PhysDistanceConstraint3D::GetFrequency() const
|
||||
{
|
||||
return GetConstraint<JPH::DistanceConstraint>()->GetLimitsSpringSettings().mFrequency;
|
||||
}
|
||||
|
||||
float PhysDistanceConstraint3D::GetMaxDistance() const
|
||||
{
|
||||
return GetConstraint<JPH::DistanceConstraint>()->GetMaxDistance();
|
||||
}
|
||||
|
||||
float PhysDistanceConstraint3D::GetMinDistance() const
|
||||
{
|
||||
return GetConstraint<JPH::DistanceConstraint>()->GetMinDistance();
|
||||
}
|
||||
|
||||
void PhysDistanceConstraint3D::SetDamping(float damping)
|
||||
{
|
||||
GetConstraint<JPH::DistanceConstraint>()->GetLimitsSpringSettings().mDamping = damping;
|
||||
}
|
||||
|
||||
void PhysDistanceConstraint3D::SetDistance(float minDist, float maxDist)
|
||||
{
|
||||
GetConstraint<JPH::DistanceConstraint>()->SetDistance(minDist, maxDist);
|
||||
}
|
||||
|
||||
void PhysDistanceConstraint3D::SetFrequency(float frequency)
|
||||
{
|
||||
GetConstraint<JPH::DistanceConstraint>()->GetLimitsSpringSettings().mFrequency = frequency;
|
||||
}
|
||||
|
||||
void PhysDistanceConstraint3D::SetMaxDistance(float maxDist)
|
||||
{
|
||||
JPH::DistanceConstraint* constraint = GetConstraint<JPH::DistanceConstraint>();
|
||||
|
||||
constraint->SetDistance(constraint->GetMinDistance(), maxDist);
|
||||
}
|
||||
|
||||
void PhysDistanceConstraint3D::SetMinDistance(float minDist)
|
||||
{
|
||||
JPH::DistanceConstraint* constraint = GetConstraint<JPH::DistanceConstraint>();
|
||||
|
||||
constraint->SetDistance(minDist, constraint->GetMaxDistance());
|
||||
}
|
||||
|
||||
|
||||
PhysPivotConstraint3D::PhysPivotConstraint3D(RigidBody3D& first, const Vector3f& pivot)
|
||||
{
|
||||
JPH::PointConstraintSettings settings;
|
||||
settings.mPoint1 = ToJolt(pivot);
|
||||
settings.mPoint2 = ToJolt(pivot);
|
||||
settings.mSpace = JPH::EConstraintSpace::WorldSpace;
|
||||
|
||||
SetupConstraint(std::make_unique<JPH::PointConstraint>(*first.GetBody(), JPH::Body::sFixedToWorld, settings));
|
||||
}
|
||||
|
||||
PhysPivotConstraint3D::PhysPivotConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& pivot)
|
||||
{
|
||||
JPH::PointConstraintSettings settings;
|
||||
settings.mPoint1 = ToJolt(pivot);
|
||||
settings.mPoint2 = ToJolt(pivot);
|
||||
settings.mSpace = JPH::EConstraintSpace::WorldSpace;
|
||||
|
||||
SetupConstraint(std::make_unique<JPH::PointConstraint>(*first.GetBody(), *second.GetBody(), settings));
|
||||
}
|
||||
|
||||
PhysPivotConstraint3D::PhysPivotConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& firstAnchor, const Vector3f& secondAnchor)
|
||||
{
|
||||
JPH::PointConstraintSettings settings;
|
||||
settings.mPoint1 = ToJolt(firstAnchor);
|
||||
settings.mPoint2 = ToJolt(secondAnchor);
|
||||
settings.mSpace = JPH::EConstraintSpace::WorldSpace;
|
||||
|
||||
SetupConstraint(std::make_unique<JPH::PointConstraint>(*first.GetBody(), *second.GetBody(), settings));
|
||||
}
|
||||
|
||||
Vector3f PhysPivotConstraint3D::GetFirstAnchor() const
|
||||
{
|
||||
const JPH::PointConstraint* constraint = GetConstraint<JPH::PointConstraint>();
|
||||
|
||||
return FromJolt(constraint->GetBody1()->GetCenterOfMassTransform() * constraint->GetLocalSpacePoint1());
|
||||
}
|
||||
|
||||
Vector3f PhysPivotConstraint3D::GetSecondAnchor() const
|
||||
{
|
||||
const JPH::PointConstraint* constraint = GetConstraint<JPH::PointConstraint>();
|
||||
|
||||
return FromJolt(constraint->GetBody2()->GetCenterOfMassTransform() * constraint->GetLocalSpacePoint2());
|
||||
}
|
||||
|
||||
void PhysPivotConstraint3D::SetFirstAnchor(const Vector3f& firstAnchor)
|
||||
{
|
||||
GetConstraint<JPH::PointConstraint>()->SetPoint1(JPH::EConstraintSpace::WorldSpace, ToJolt(firstAnchor));
|
||||
GetConstraint<JPH::PointConstraint>()->SetPoint2(JPH::EConstraintSpace::WorldSpace, ToJolt(firstAnchor));
|
||||
}
|
||||
|
||||
void PhysPivotConstraint3D::SetSecondAnchor(const Vector3f& secondAnchor)
|
||||
{
|
||||
GetConstraint<JPH::PointConstraint>()->SetPoint2(JPH::EConstraintSpace::WorldSpace, ToJolt(secondAnchor));
|
||||
}
|
||||
}
|
||||
683
src/Nazara/Physics3D/PhysWorld3D.cpp
Normal file
683
src/Nazara/Physics3D/PhysWorld3D.cpp
Normal file
@@ -0,0 +1,683 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/PhysWorld3D.hpp>
|
||||
#include <Nazara/Physics3D/Collider3D.hpp>
|
||||
#include <Nazara/Physics3D/JoltHelper.hpp>
|
||||
#include <Nazara/Physics3D/PhysCharacter3D.hpp>
|
||||
#include <Nazara/Physics3D/Physics3D.hpp>
|
||||
#include <Nazara/Physics3D/Physiscs3DStepListener.hpp>
|
||||
#include <NazaraUtils/MemoryPool.hpp>
|
||||
#include <NazaraUtils/StackVector.hpp>
|
||||
#include <Jolt/Jolt.h>
|
||||
#include <Jolt/Core/TempAllocator.h>
|
||||
#include <Jolt/Physics/PhysicsSettings.h>
|
||||
#include <Jolt/Physics/PhysicsStepListener.h>
|
||||
#include <Jolt/Physics/PhysicsSystem.h>
|
||||
#include <Jolt/Physics/Body/BodyActivationListener.h>
|
||||
#include <Jolt/Physics/Collision/CastResult.h>
|
||||
#include <Jolt/Physics/Collision/CollidePointResult.h>
|
||||
#include <Jolt/Physics/Collision/CollideShape.h>
|
||||
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
|
||||
#include <Jolt/Physics/Collision/ObjectLayer.h>
|
||||
#include <Jolt/Physics/Collision/RayCast.h>
|
||||
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
|
||||
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
|
||||
#include <tsl/ordered_set.h>
|
||||
#include <cassert>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace DitchMeAsap
|
||||
{
|
||||
using namespace JPH;
|
||||
|
||||
// Layer that objects can be in, determines which other objects it can collide with
|
||||
// Typically you at least want to have 1 layer for moving bodies and 1 layer for static bodies, but you can have more
|
||||
// layers if you want. E.g. you could have a layer for high detail collision (which is not used by the physics simulation
|
||||
// but only if you do collision testing).
|
||||
namespace Layers
|
||||
{
|
||||
static constexpr uint8 NON_MOVING = 0;
|
||||
static constexpr uint8 MOVING = 1;
|
||||
static constexpr uint8 NUM_LAYERS = 2;
|
||||
};
|
||||
|
||||
/// Class that determines if two object layers can collide
|
||||
class ObjectLayerPairFilterImpl : public ObjectLayerPairFilter
|
||||
{
|
||||
public:
|
||||
virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
|
||||
{
|
||||
switch (inObject1)
|
||||
{
|
||||
case Layers::NON_MOVING:
|
||||
return inObject2 == Layers::MOVING; // Non moving only collides with moving
|
||||
case Layers::MOVING:
|
||||
return true; // Moving collides with everything
|
||||
default:
|
||||
JPH_ASSERT(false);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Each broadphase layer results in a separate bounding volume tree in the broad phase. You at least want to have
|
||||
// a layer for non-moving and moving objects to avoid having to update a tree full of static objects every frame.
|
||||
// You can have a 1-on-1 mapping between object layers and broadphase layers (like in this case) but if you have
|
||||
// many object layers you'll be creating many broad phase trees, which is not efficient. If you want to fine tune
|
||||
// your broadphase layers define JPH_TRACK_BROADPHASE_STATS and look at the stats reported on the TTY.
|
||||
namespace BroadPhaseLayers
|
||||
{
|
||||
static constexpr BroadPhaseLayer NON_MOVING(0);
|
||||
static constexpr BroadPhaseLayer MOVING(1);
|
||||
static constexpr uint NUM_LAYERS(2);
|
||||
};
|
||||
|
||||
// BroadPhaseLayerInterface implementation
|
||||
// This defines a mapping between object and broadphase layers.
|
||||
class BPLayerInterfaceImpl final : public BroadPhaseLayerInterface
|
||||
{
|
||||
public:
|
||||
BPLayerInterfaceImpl()
|
||||
{
|
||||
// Create a mapping table from object to broad phase layer
|
||||
mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
|
||||
mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
|
||||
}
|
||||
|
||||
virtual uint GetNumBroadPhaseLayers() const override
|
||||
{
|
||||
return BroadPhaseLayers::NUM_LAYERS;
|
||||
}
|
||||
|
||||
virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override
|
||||
{
|
||||
JPH_ASSERT(inLayer < Layers::NUM_LAYERS);
|
||||
return mObjectToBroadPhase[inLayer];
|
||||
}
|
||||
|
||||
#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
|
||||
virtual const char* GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
|
||||
{
|
||||
switch ((BroadPhaseLayer::Type)inLayer)
|
||||
{
|
||||
case (BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING: return "NON_MOVING";
|
||||
case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING: return "MOVING";
|
||||
default: JPH_ASSERT(false); return "INVALID";
|
||||
}
|
||||
}
|
||||
#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
|
||||
|
||||
private:
|
||||
BroadPhaseLayer mObjectToBroadPhase[Layers::NUM_LAYERS];
|
||||
};
|
||||
/// Class that determines if an object layer can collide with a broadphase layer
|
||||
class ObjectVsBroadPhaseLayerFilterImpl : public ObjectVsBroadPhaseLayerFilter
|
||||
{
|
||||
public:
|
||||
virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
|
||||
{
|
||||
switch (inLayer1)
|
||||
{
|
||||
case Layers::NON_MOVING:
|
||||
return inLayer2 == BroadPhaseLayers::MOVING;
|
||||
case Layers::MOVING:
|
||||
return true;
|
||||
default:
|
||||
JPH_ASSERT(false);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// An example contact listener
|
||||
/*class MyContactListener : public ContactListener
|
||||
{
|
||||
public:
|
||||
// See: ContactListener
|
||||
virtual ValidateResult OnContactValidate(const Body& inBody1, const Body& inBody2, RVec3Arg inBaseOffset, const CollideShapeResult& inCollisionResult) override
|
||||
{
|
||||
cout << "Contact validate callback" << endl;
|
||||
|
||||
// Allows you to ignore a contact before it is created (using layers to not make objects collide is cheaper!)
|
||||
return ValidateResult::AcceptAllContactsForThisBodyPair;
|
||||
}
|
||||
|
||||
virtual void OnContactAdded(const Body& inBody1, const Body& inBody2, const ContactManifold& inManifold, ContactSettings& ioSettings) override
|
||||
{
|
||||
cout << "A contact was added" << endl;
|
||||
}
|
||||
|
||||
virtual void OnContactPersisted(const Body& inBody1, const Body& inBody2, const ContactManifold& inManifold, ContactSettings& ioSettings) override
|
||||
{
|
||||
cout << "A contact was persisted" << endl;
|
||||
}
|
||||
|
||||
virtual void OnContactRemoved(const SubShapeIDPair& inSubShapePair) override
|
||||
{
|
||||
cout << "A contact was removed" << endl;
|
||||
}
|
||||
};*/
|
||||
}
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
namespace NAZARA_ANONYMOUS_NAMESPACE
|
||||
{
|
||||
class PointCallbackHitResult : public JPH::CollidePointCollector
|
||||
{
|
||||
public:
|
||||
PointCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const FunctionRef<std::optional<float>(const PhysWorld3D::PointCollisionInfo& hitInfo)>& callback) :
|
||||
m_bodyLockInterface(bodyLockInterface),
|
||||
m_callback(callback),
|
||||
m_didHit(false)
|
||||
{
|
||||
}
|
||||
|
||||
void AddHit(const JPH::CollidePointResult& result) override
|
||||
{
|
||||
PhysWorld3D::PointCollisionInfo hitInfo;
|
||||
|
||||
JPH::BodyLockWrite lock(m_bodyLockInterface, result.mBodyID);
|
||||
if (!lock.Succeeded())
|
||||
return; //< body was destroyed
|
||||
|
||||
JPH::Body& body = lock.GetBody();
|
||||
|
||||
hitInfo.hitBody = BitCast<Physics3DBody*>(static_cast<std::uintptr_t>(body.GetUserData()));
|
||||
|
||||
if (auto fractionOpt = m_callback(hitInfo))
|
||||
{
|
||||
float fraction = fractionOpt.value();
|
||||
if (fraction > 0.f)
|
||||
{
|
||||
m_didHit = true;
|
||||
UpdateEarlyOutFraction(fraction);
|
||||
}
|
||||
else
|
||||
ForceEarlyOut();
|
||||
}
|
||||
}
|
||||
|
||||
bool DidHit() const
|
||||
{
|
||||
return m_didHit;
|
||||
}
|
||||
|
||||
private:
|
||||
const JPH::BodyLockInterface& m_bodyLockInterface;
|
||||
const FunctionRef<std::optional<float>(const PhysWorld3D::PointCollisionInfo& hitInfo)>& m_callback;
|
||||
bool m_didHit;
|
||||
};
|
||||
|
||||
class ShapeCallbackHitResult : public JPH::CollideShapeCollector
|
||||
{
|
||||
public:
|
||||
ShapeCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const FunctionRef<std::optional<float>(const PhysWorld3D::ShapeCollisionInfo& hitInfo)>& callback) :
|
||||
m_bodyLockInterface(bodyLockInterface),
|
||||
m_callback(callback),
|
||||
m_didHit(false)
|
||||
{
|
||||
}
|
||||
|
||||
void AddHit(const JPH::CollideShapeResult& result) override
|
||||
{
|
||||
PhysWorld3D::ShapeCollisionInfo hitInfo;
|
||||
hitInfo.collisionPosition1 = FromJolt(result.mContactPointOn1);
|
||||
hitInfo.collisionPosition2 = FromJolt(result.mContactPointOn2);
|
||||
hitInfo.penetrationAxis = FromJolt(result.mPenetrationAxis);
|
||||
hitInfo.penetrationDepth = result.mPenetrationDepth;
|
||||
|
||||
JPH::BodyLockWrite lock(m_bodyLockInterface, result.mBodyID2);
|
||||
if (!lock.Succeeded())
|
||||
return; //< body was destroyed
|
||||
|
||||
JPH::Body& body = lock.GetBody();
|
||||
|
||||
hitInfo.hitBody = BitCast<Physics3DBody*>(static_cast<std::uintptr_t>(body.GetUserData()));
|
||||
|
||||
if (auto fractionOpt = m_callback(hitInfo))
|
||||
{
|
||||
float fraction = fractionOpt.value();
|
||||
if (fraction > 0.f)
|
||||
{
|
||||
m_didHit = true;
|
||||
UpdateEarlyOutFraction(fraction);
|
||||
}
|
||||
else
|
||||
ForceEarlyOut();
|
||||
}
|
||||
}
|
||||
|
||||
bool DidHit() const
|
||||
{
|
||||
return m_didHit;
|
||||
}
|
||||
|
||||
private:
|
||||
const JPH::BodyLockInterface& m_bodyLockInterface;
|
||||
const FunctionRef<std::optional<float>(const PhysWorld3D::ShapeCollisionInfo& hitInfo)>& m_callback;
|
||||
bool m_didHit;
|
||||
};
|
||||
|
||||
class RaycastCallbackHitResult : public JPH::CastRayCollector
|
||||
{
|
||||
public:
|
||||
RaycastCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const Vector3f& from, const Vector3f& to, const FunctionRef<std::optional<float>(const PhysWorld3D::RaycastHit& hitInfo)>& callback) :
|
||||
m_bodyLockInterface(bodyLockInterface),
|
||||
m_callback(callback),
|
||||
m_from(from),
|
||||
m_to(to),
|
||||
m_didHit(false)
|
||||
{
|
||||
}
|
||||
|
||||
void AddHit(const JPH::RayCastResult& result) override
|
||||
{
|
||||
PhysWorld3D::RaycastHit hitInfo;
|
||||
hitInfo.fraction = result.mFraction;
|
||||
hitInfo.hitPosition = Lerp(m_from, m_to, result.mFraction);
|
||||
|
||||
JPH::BodyLockWrite lock(m_bodyLockInterface, result.mBodyID);
|
||||
if (!lock.Succeeded())
|
||||
return; //< body was destroyed
|
||||
|
||||
JPH::Body& body = lock.GetBody();
|
||||
|
||||
hitInfo.hitBody = BitCast<Physics3DBody*>(static_cast<std::uintptr_t>(body.GetUserData()));
|
||||
hitInfo.hitNormal = FromJolt(body.GetWorldSpaceSurfaceNormal(result.mSubShapeID2, ToJolt(hitInfo.hitPosition)));
|
||||
|
||||
if (auto fractionOpt = m_callback(hitInfo))
|
||||
{
|
||||
float fraction = fractionOpt.value();
|
||||
if (fraction > 0.f)
|
||||
{
|
||||
m_didHit = true;
|
||||
UpdateEarlyOutFraction(fraction);
|
||||
}
|
||||
else
|
||||
ForceEarlyOut();
|
||||
}
|
||||
}
|
||||
|
||||
bool DidHit() const
|
||||
{
|
||||
return m_didHit;
|
||||
}
|
||||
|
||||
private:
|
||||
const JPH::BodyLockInterface& m_bodyLockInterface;
|
||||
const FunctionRef<std::optional<float>(const PhysWorld3D::RaycastHit& hitInfo)>& m_callback;
|
||||
Vector3f m_from;
|
||||
Vector3f m_to;
|
||||
bool m_didHit;
|
||||
};
|
||||
}
|
||||
|
||||
class PhysWorld3D::BodyActivationListener : public JPH::BodyActivationListener
|
||||
{
|
||||
public:
|
||||
static constexpr UInt32 BodyPerBlock = 64;
|
||||
|
||||
BodyActivationListener(PhysWorld3D& physWorld) :
|
||||
m_physWorld(physWorld)
|
||||
{
|
||||
}
|
||||
|
||||
void OnBodyActivated(const JPH::BodyID& inBodyID, UInt64 /*inBodyUserData*/) override
|
||||
{
|
||||
UInt32 bodyIndex = inBodyID.GetIndex();
|
||||
UInt32 blockIndex = bodyIndex / 64;
|
||||
UInt32 localIndex = bodyIndex % 64;
|
||||
|
||||
m_physWorld.m_activeBodies[blockIndex] |= UInt64(1u) << localIndex;
|
||||
}
|
||||
|
||||
void OnBodyDeactivated(const JPH::BodyID& inBodyID, UInt64 /*inBodyUserData*/) override
|
||||
{
|
||||
UInt32 bodyIndex = inBodyID.GetIndex();
|
||||
UInt32 blockIndex = bodyIndex / 64;
|
||||
UInt32 localIndex = bodyIndex % 64;
|
||||
|
||||
m_physWorld.m_activeBodies[blockIndex] &= ~(UInt64(1u) << localIndex);
|
||||
}
|
||||
|
||||
private:
|
||||
PhysWorld3D& m_physWorld;
|
||||
};
|
||||
|
||||
class PhysWorld3D::StepListener : public JPH::PhysicsStepListener
|
||||
{
|
||||
public:
|
||||
StepListener(PhysWorld3D& physWorld) :
|
||||
m_physWorld(physWorld)
|
||||
{
|
||||
}
|
||||
|
||||
void OnStep(float inDeltaTime, JPH::PhysicsSystem& /*inPhysicsSystem*/) override
|
||||
{
|
||||
m_physWorld.OnPreStep(inDeltaTime);
|
||||
}
|
||||
|
||||
private:
|
||||
PhysWorld3D& m_physWorld;
|
||||
};
|
||||
|
||||
struct PhysWorld3D::JoltWorld
|
||||
{
|
||||
using BodySet = tsl::ordered_set<JPH::BodyID, std::hash<JPH::BodyID>, std::equal_to<JPH::BodyID>, std::allocator<JPH::BodyID>, std::vector<JPH::BodyID>>;
|
||||
|
||||
JPH::TempAllocatorImpl tempAllocator;
|
||||
JPH::PhysicsSystem physicsSystem;
|
||||
BodySet pendingAdditionActivate;
|
||||
BodySet pendingAdditionNoActivate;
|
||||
BodySet pendingDeactivations;
|
||||
std::vector<JPH::BodyID> tempBodyIDVec;
|
||||
std::unique_ptr<JPH::SphereShape> nullShape;
|
||||
|
||||
PhysWorld3D::BodyActivationListener bodyActivationListener;
|
||||
PhysWorld3D::StepListener stepListener;
|
||||
|
||||
DitchMeAsap::BPLayerInterfaceImpl layerInterface;
|
||||
DitchMeAsap::ObjectLayerPairFilterImpl objectLayerFilter;
|
||||
DitchMeAsap::ObjectVsBroadPhaseLayerFilterImpl objectBroadphaseLayerFilter;
|
||||
|
||||
JoltWorld(PhysWorld3D& world, JPH::uint tempAllocatorSize) :
|
||||
tempAllocator(tempAllocatorSize),
|
||||
bodyActivationListener(world),
|
||||
stepListener(world)
|
||||
{
|
||||
}
|
||||
|
||||
JoltWorld(const JoltWorld&) = delete;
|
||||
JoltWorld(JoltWorld&&) = delete;
|
||||
|
||||
JoltWorld& operator=(const JoltWorld&) = delete;
|
||||
JoltWorld& operator=(JoltWorld&&) = delete;
|
||||
};
|
||||
|
||||
PhysWorld3D::PhysWorld3D() :
|
||||
m_maxStepCount(50),
|
||||
m_gravity(Vector3f::Zero()),
|
||||
m_stepSize(Time::TickDuration(120)),
|
||||
m_timestepAccumulator(Time::Zero())
|
||||
{
|
||||
m_world = std::make_unique<JoltWorld>(*this, 10 * 1024 * 1024);
|
||||
m_world->physicsSystem.Init(0xFFFF, 0, 0xFFFF, 10 * 1024, m_world->layerInterface, m_world->objectBroadphaseLayerFilter, m_world->objectLayerFilter);
|
||||
m_world->physicsSystem.SetBodyActivationListener(&m_world->bodyActivationListener);
|
||||
|
||||
m_world->physicsSystem.AddStepListener(&m_world->stepListener);
|
||||
|
||||
std::size_t blockCount = (m_world->physicsSystem.GetMaxBodies() - 1) / 64 + 1;
|
||||
|
||||
m_activeBodies = std::make_unique<std::atomic_uint64_t[]>(blockCount);
|
||||
for (std::size_t i = 0; i < blockCount; ++i)
|
||||
m_activeBodies[i] = 0;
|
||||
|
||||
m_registeredBodies = std::make_unique<std::uint64_t[]>(blockCount);
|
||||
for (std::size_t i = 0; i < blockCount; ++i)
|
||||
m_registeredBodies[i] = 0;
|
||||
}
|
||||
|
||||
PhysWorld3D::~PhysWorld3D() = default;
|
||||
|
||||
bool PhysWorld3D::CollisionQuery(const Vector3f& point, const FunctionRef<std::optional<float>(const PointCollisionInfo& collisionInfo)>& callback)
|
||||
{
|
||||
NAZARA_USE_ANONYMOUS_NAMESPACE
|
||||
|
||||
PointCallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), callback);
|
||||
m_world->physicsSystem.GetNarrowPhaseQuery().CollidePoint(ToJolt(point), collector);
|
||||
|
||||
return collector.DidHit();
|
||||
}
|
||||
|
||||
bool PhysWorld3D::CollisionQuery(const Collider3D& collider, const Matrix4f& colliderTransform, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
|
||||
{
|
||||
return CollisionQuery(collider, colliderTransform, Vector3f::Unit(), callback);
|
||||
}
|
||||
|
||||
bool PhysWorld3D::CollisionQuery(const Collider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
|
||||
{
|
||||
NAZARA_USE_ANONYMOUS_NAMESPACE
|
||||
|
||||
JPH::Shape* shape = collider.GetShapeSettings()->Create().Get();
|
||||
|
||||
JPH::CollideShapeSettings collideShapeSettings;
|
||||
|
||||
ShapeCallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), callback);
|
||||
m_world->physicsSystem.GetNarrowPhaseQuery().CollideShape(shape, ToJolt(colliderScale), ToJolt(colliderTransform), collideShapeSettings, JPH::Vec3::sZero(), collector);
|
||||
|
||||
return collector.DidHit();
|
||||
}
|
||||
|
||||
UInt32 PhysWorld3D::GetActiveBodyCount() const
|
||||
{
|
||||
return m_world->physicsSystem.GetNumActiveBodies(JPH::EBodyType::RigidBody);
|
||||
}
|
||||
|
||||
Vector3f PhysWorld3D::GetGravity() const
|
||||
{
|
||||
return FromJolt(m_world->physicsSystem.GetGravity());
|
||||
}
|
||||
|
||||
std::size_t PhysWorld3D::GetMaxStepCount() const
|
||||
{
|
||||
return m_maxStepCount;
|
||||
}
|
||||
|
||||
JPH::PhysicsSystem* PhysWorld3D::GetPhysicsSystem()
|
||||
{
|
||||
return &m_world->physicsSystem;
|
||||
}
|
||||
|
||||
Time PhysWorld3D::GetStepSize() const
|
||||
{
|
||||
return m_stepSize;
|
||||
}
|
||||
|
||||
bool PhysWorld3D::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef<std::optional<float>(const RaycastHit& hitInfo)>& callback)
|
||||
{
|
||||
NAZARA_USE_ANONYMOUS_NAMESPACE
|
||||
|
||||
JPH::RRayCast rayCast;
|
||||
rayCast.mDirection = ToJolt(to - from);
|
||||
rayCast.mOrigin = ToJolt(from);
|
||||
|
||||
JPH::RayCastSettings rayCastSettings;
|
||||
|
||||
RaycastCallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), from, to, callback);
|
||||
m_world->physicsSystem.GetNarrowPhaseQuery().CastRay(rayCast, rayCastSettings, collector);
|
||||
|
||||
return collector.DidHit();
|
||||
}
|
||||
|
||||
bool PhysWorld3D::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef<void(const RaycastHit& hitInfo)>& callback)
|
||||
{
|
||||
JPH::RRayCast rayCast;
|
||||
rayCast.mDirection = ToJolt(to - from);
|
||||
rayCast.mOrigin = ToJolt(from);
|
||||
|
||||
JPH::RayCastSettings rayCastSettings;
|
||||
|
||||
JPH::ClosestHitCollisionCollector<JPH::CastRayCollector> collector;
|
||||
m_world->physicsSystem.GetNarrowPhaseQuery().CastRay(rayCast, rayCastSettings, collector);
|
||||
|
||||
if (!collector.HadHit())
|
||||
return false;
|
||||
|
||||
JPH::BodyLockWrite lock(m_world->physicsSystem.GetBodyLockInterface(), collector.mHit.mBodyID);
|
||||
if (!lock.Succeeded())
|
||||
return false; //< body was destroyed before lock
|
||||
|
||||
JPH::Body& body = lock.GetBody();
|
||||
|
||||
RaycastHit hitInfo;
|
||||
hitInfo.fraction = collector.mHit.GetEarlyOutFraction();
|
||||
hitInfo.hitPosition = Lerp(from, to, hitInfo.fraction);
|
||||
hitInfo.hitBody = BitCast<Physics3DBody*>(static_cast<std::uintptr_t>(body.GetUserData()));
|
||||
hitInfo.hitNormal = FromJolt(body.GetWorldSpaceSurfaceNormal(collector.mHit.mSubShapeID2, rayCast.GetPointOnRay(collector.mHit.GetEarlyOutFraction())));
|
||||
|
||||
callback(hitInfo);
|
||||
return true;
|
||||
}
|
||||
|
||||
void PhysWorld3D::RefreshBodies()
|
||||
{
|
||||
// Batch add bodies (keeps the broadphase efficient)
|
||||
JPH::BodyInterface& bodyInterface = m_world->physicsSystem.GetBodyInterfaceNoLock();
|
||||
auto AddBodies = [&](const JoltWorld::BodySet& bodies, JPH::EActivation activation)
|
||||
{
|
||||
for (const JPH::BodyID& bodyId : bodies)
|
||||
{
|
||||
UInt32 bodyIndex = bodyId.GetIndex();
|
||||
UInt32 blockIndex = bodyIndex / 64;
|
||||
UInt32 localIndex = bodyIndex % 64;
|
||||
|
||||
m_registeredBodies[blockIndex] |= UInt64(1u) << localIndex;
|
||||
}
|
||||
|
||||
if (bodies.size() == 1)
|
||||
bodyInterface.AddBody(bodies.front(), activation);
|
||||
else
|
||||
{
|
||||
m_world->tempBodyIDVec.resize(bodies.size());
|
||||
std::memcpy(&m_world->tempBodyIDVec[0], bodies.data(), bodies.size() * sizeof(JPH::BodyID));
|
||||
|
||||
JPH::BodyInterface::AddState addState = bodyInterface.AddBodiesPrepare(m_world->tempBodyIDVec.data(), SafeCast<int>(m_world->tempBodyIDVec.size()));
|
||||
bodyInterface.AddBodiesFinalize(m_world->tempBodyIDVec.data(), SafeCast<int>(m_world->tempBodyIDVec.size()), addState, activation);
|
||||
}
|
||||
};
|
||||
|
||||
// Handle pending register/unregister bodies
|
||||
if (!m_world->pendingAdditionActivate.empty())
|
||||
{
|
||||
AddBodies(m_world->pendingAdditionActivate, JPH::EActivation::Activate);
|
||||
m_world->pendingAdditionActivate.clear();
|
||||
}
|
||||
|
||||
if (!m_world->pendingAdditionNoActivate.empty())
|
||||
{
|
||||
AddBodies(m_world->pendingAdditionNoActivate, JPH::EActivation::DontActivate);
|
||||
m_world->pendingAdditionNoActivate.clear();
|
||||
}
|
||||
|
||||
if (!m_world->pendingDeactivations.empty())
|
||||
{
|
||||
bodyInterface.DeactivateBodies(m_world->pendingDeactivations.data(), SafeCast<int>(m_world->pendingDeactivations.size()));
|
||||
m_world->pendingDeactivations.clear();
|
||||
}
|
||||
}
|
||||
|
||||
void PhysWorld3D::SetGravity(const Vector3f& gravity)
|
||||
{
|
||||
m_world->physicsSystem.SetGravity(ToJolt(gravity));
|
||||
}
|
||||
|
||||
void PhysWorld3D::SetMaxStepCount(std::size_t maxStepCount)
|
||||
{
|
||||
m_maxStepCount = maxStepCount;
|
||||
}
|
||||
|
||||
void PhysWorld3D::SetStepSize(Time stepSize)
|
||||
{
|
||||
m_stepSize = stepSize;
|
||||
}
|
||||
|
||||
bool PhysWorld3D::Step(Time timestep)
|
||||
{
|
||||
m_timestepAccumulator += timestep;
|
||||
if (m_timestepAccumulator < m_stepSize)
|
||||
return false;
|
||||
|
||||
RefreshBodies();
|
||||
|
||||
JPH::JobSystem& jobSystem = Physics3D::Instance()->GetThreadPool();
|
||||
float stepSize = m_stepSize.AsSeconds<float>();
|
||||
|
||||
std::size_t stepCount = 0;
|
||||
while (m_timestepAccumulator >= m_stepSize && stepCount < m_maxStepCount)
|
||||
{
|
||||
m_world->physicsSystem.Update(stepSize, 1, &m_world->tempAllocator, &jobSystem);
|
||||
|
||||
for (Physiscs3DStepListener* stepListener : m_stepListeners)
|
||||
stepListener->PostSimulate(stepSize);
|
||||
|
||||
m_timestepAccumulator -= m_stepSize;
|
||||
stepCount++;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PhysWorld3D::RegisterBody(const JPH::BodyID& bodyID, bool activate, bool removeFromDeactivationList)
|
||||
{
|
||||
assert(removeFromDeactivationList || !m_world->pendingDeactivations.contains(bodyID));
|
||||
|
||||
auto& activationSet = (activate) ? m_world->pendingAdditionActivate : m_world->pendingAdditionNoActivate;
|
||||
activationSet.insert(bodyID);
|
||||
|
||||
if (removeFromDeactivationList)
|
||||
{
|
||||
auto& otherActivationSet = (activate) ? m_world->pendingAdditionNoActivate : m_world->pendingAdditionActivate;
|
||||
otherActivationSet.erase(bodyID);
|
||||
|
||||
m_world->pendingDeactivations.erase(bodyID);
|
||||
}
|
||||
}
|
||||
|
||||
void PhysWorld3D::UnregisterBody(const JPH::BodyID& bodyID, bool destroy, bool removeFromActivationList)
|
||||
{
|
||||
// Remove from list first as bodyID may be invalidated by destruction
|
||||
if (removeFromActivationList)
|
||||
{
|
||||
m_world->pendingAdditionActivate.erase(bodyID);
|
||||
m_world->pendingAdditionNoActivate.erase(bodyID);
|
||||
}
|
||||
|
||||
if (destroy)
|
||||
{
|
||||
auto& bodyInterface = m_world->physicsSystem.GetBodyInterface();
|
||||
|
||||
UInt32 bodyIndex = bodyID.GetIndex();
|
||||
if (IsBodyRegistered(bodyIndex))
|
||||
{
|
||||
UInt32 blockIndex = bodyIndex / 64;
|
||||
UInt32 localIndex = bodyIndex % 64;
|
||||
|
||||
m_registeredBodies[blockIndex] &= ~(UInt64(1u) << localIndex);
|
||||
|
||||
bodyInterface.RemoveBody(bodyID);
|
||||
}
|
||||
|
||||
bodyInterface.DestroyBody(bodyID); //< this invalidate the bodyID reference!
|
||||
}
|
||||
else
|
||||
m_world->pendingDeactivations.insert(bodyID);
|
||||
}
|
||||
|
||||
std::shared_ptr<PhysCharacter3DImpl> PhysWorld3D::GetDefaultCharacterImpl()
|
||||
{
|
||||
if (!m_defaultCharacterImpl)
|
||||
m_defaultCharacterImpl = std::make_shared<PhysCharacter3DImpl>();
|
||||
|
||||
return m_defaultCharacterImpl;
|
||||
}
|
||||
|
||||
const JPH::Shape* PhysWorld3D::GetNullShape() const
|
||||
{
|
||||
if (!m_world->nullShape)
|
||||
{
|
||||
m_world->nullShape = std::make_unique<JPH::SphereShape>(std::numeric_limits<float>::epsilon());
|
||||
m_world->nullShape->SetEmbedded();
|
||||
}
|
||||
|
||||
return m_world->nullShape.get();
|
||||
}
|
||||
|
||||
void PhysWorld3D::OnPreStep(float deltatime)
|
||||
{
|
||||
for (Physiscs3DStepListener* stepListener : m_stepListeners)
|
||||
stepListener->PreSimulate(deltatime);
|
||||
}
|
||||
}
|
||||
65
src/Nazara/Physics3D/Physics3D.cpp
Normal file
65
src/Nazara/Physics3D/Physics3D.cpp
Normal file
@@ -0,0 +1,65 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/Physics3D.hpp>
|
||||
#include <Nazara/Core/Core.hpp>
|
||||
#include <Nazara/Core/Error.hpp>
|
||||
#include <Nazara/Core/Log.hpp>
|
||||
#include <Nazara/Physics3D/Config.hpp>
|
||||
#include <Jolt/Jolt.h>
|
||||
#include <Jolt/RegisterTypes.h>
|
||||
#include <Jolt/Core/Factory.h>
|
||||
#include <Jolt/Core/JobSystemThreadPool.h>
|
||||
#include <Jolt/Physics/PhysicsSettings.h>
|
||||
#include <cstdarg>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
// Callback for traces, connect this to your own trace function if you have one
|
||||
static void TraceImpl(const char* inFMT, ...)
|
||||
{
|
||||
// Format the message
|
||||
va_list list;
|
||||
va_start(list, inFMT);
|
||||
char buffer[1024];
|
||||
vsnprintf(buffer, sizeof(buffer), inFMT, list);
|
||||
va_end(list);
|
||||
|
||||
// Print to the TTY
|
||||
NazaraError(buffer);
|
||||
}
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
Physics3D::Physics3D(Config /*config*/) :
|
||||
ModuleBase("Physics3D", this)
|
||||
{
|
||||
JPH::RegisterDefaultAllocator();
|
||||
JPH::Trace = TraceImpl;
|
||||
JPH::Factory::sInstance = new JPH::Factory;
|
||||
JPH::RegisterTypes();
|
||||
|
||||
int threadCount = -1; //< system CPU core count
|
||||
#ifdef NAZARA_PLATFORM_WEB
|
||||
threadCount = 0; // no thread on web for now
|
||||
#endif
|
||||
|
||||
m_threadPool = std::make_unique<JPH::JobSystemThreadPool>(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, threadCount);
|
||||
}
|
||||
|
||||
Physics3D::~Physics3D()
|
||||
{
|
||||
m_threadPool.reset();
|
||||
JPH::UnregisterTypes();
|
||||
|
||||
delete JPH::Factory::sInstance;
|
||||
JPH::Factory::sInstance = nullptr;
|
||||
}
|
||||
|
||||
JPH::JobSystem& Physics3D::GetThreadPool()
|
||||
{
|
||||
return *m_threadPool;
|
||||
}
|
||||
|
||||
Physics3D* Physics3D::s_instance;
|
||||
}
|
||||
11
src/Nazara/Physics3D/Physics3DBody.cpp
Normal file
11
src/Nazara/Physics3D/Physics3DBody.cpp
Normal file
@@ -0,0 +1,11 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/Physics3DBody.hpp>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
Physics3DBody::~Physics3DBody() = default;
|
||||
}
|
||||
19
src/Nazara/Physics3D/Physiscs3DStepListener.cpp
Normal file
19
src/Nazara/Physics3D/Physiscs3DStepListener.cpp
Normal file
@@ -0,0 +1,19 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/Physiscs3DStepListener.hpp>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
Physiscs3DStepListener::~Physiscs3DStepListener() = default;
|
||||
|
||||
void Physiscs3DStepListener::PostSimulate(float /*elapsedTime*/)
|
||||
{
|
||||
}
|
||||
|
||||
void Physiscs3DStepListener::PreSimulate(float /*elapsedTime*/)
|
||||
{
|
||||
}
|
||||
}
|
||||
441
src/Nazara/Physics3D/RigidBody3D.cpp
Normal file
441
src/Nazara/Physics3D/RigidBody3D.cpp
Normal file
@@ -0,0 +1,441 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/RigidBody3D.hpp>
|
||||
#include <Nazara/Physics3D/JoltHelper.hpp>
|
||||
#include <Nazara/Physics3D/PhysWorld3D.hpp>
|
||||
#include <NazaraUtils/MemoryHelper.hpp>
|
||||
#include <Jolt/Jolt.h>
|
||||
#include <Jolt/Physics/PhysicsSystem.h>
|
||||
#include <Jolt/Physics/Body/BodyCreationSettings.h>
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
RigidBody3D::RigidBody3D(RigidBody3D&& body) noexcept :
|
||||
m_geom(std::move(body.m_geom)),
|
||||
m_body(std::move(body.m_body)),
|
||||
m_world(std::move(body.m_world)),
|
||||
m_bodyIndex(body.m_bodyIndex),
|
||||
m_isSimulationEnabled(body.m_isSimulationEnabled),
|
||||
m_isTrigger(body.m_isTrigger)
|
||||
{
|
||||
body.m_bodyIndex = std::numeric_limits<UInt32>::max();
|
||||
|
||||
if (m_body)
|
||||
m_body->SetUserData(SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
}
|
||||
|
||||
RigidBody3D::~RigidBody3D()
|
||||
{
|
||||
Destroy();
|
||||
}
|
||||
|
||||
void RigidBody3D::AddForce(const Vector3f& force, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
m_body->AddForce(ToJolt(force));
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
m_body->AddForce(m_body->GetRotation() * ToJolt(force));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RigidBody3D::AddForce(const Vector3f& force, const Vector3f& point, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
m_body->AddForce(ToJolt(force), ToJolt(point));
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
m_body->AddForce(m_body->GetRotation() * ToJolt(force), ToJolt(ToWorld(point)));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RigidBody3D::AddTorque(const Vector3f& torque, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
m_body->AddTorque(ToJolt(torque));
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
m_body->AddTorque(m_body->GetRotation() * ToJolt(torque));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RigidBody3D::EnableSimulation(bool enable)
|
||||
{
|
||||
if (m_isSimulationEnabled == enable)
|
||||
return;
|
||||
|
||||
if (enable)
|
||||
m_world->RegisterBody(m_body->GetID(), true, true);
|
||||
else
|
||||
m_world->UnregisterBody(m_body->GetID(), false, true);
|
||||
|
||||
m_isSimulationEnabled = enable;
|
||||
}
|
||||
|
||||
void RigidBody3D::EnableSleeping(bool enable)
|
||||
{
|
||||
m_body->SetAllowSleeping(enable);
|
||||
}
|
||||
|
||||
void RigidBody3D::FallAsleep()
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
|
||||
bodyInterface.DeactivateBody(m_body->GetID());
|
||||
}
|
||||
|
||||
Boxf RigidBody3D::GetAABB() const
|
||||
{
|
||||
const JPH::AABox& aabb = m_body->GetWorldSpaceBounds();
|
||||
return Boxf(FromJolt(aabb.mMin), FromJolt(aabb.GetSize()));
|
||||
}
|
||||
|
||||
float RigidBody3D::GetAngularDamping() const
|
||||
{
|
||||
if NAZARA_UNLIKELY(IsStatic())
|
||||
return 0.f;
|
||||
|
||||
return m_body->GetMotionProperties()->GetAngularDamping();
|
||||
}
|
||||
|
||||
Vector3f RigidBody3D::GetAngularVelocity() const
|
||||
{
|
||||
return FromJolt(m_body->GetAngularVelocity());
|
||||
}
|
||||
|
||||
UInt32 RigidBody3D::GetBodyIndex() const
|
||||
{
|
||||
return m_bodyIndex;
|
||||
}
|
||||
|
||||
float RigidBody3D::GetLinearDamping() const
|
||||
{
|
||||
if NAZARA_UNLIKELY(IsStatic())
|
||||
return 0.f;
|
||||
|
||||
return m_body->GetMotionProperties()->GetLinearDamping();
|
||||
}
|
||||
|
||||
Vector3f RigidBody3D::GetLinearVelocity() const
|
||||
{
|
||||
return FromJolt(m_body->GetLinearVelocity());
|
||||
}
|
||||
|
||||
float RigidBody3D::GetMass() const
|
||||
{
|
||||
if NAZARA_UNLIKELY(IsStatic())
|
||||
return 0.f;
|
||||
|
||||
return 1.f / m_body->GetMotionProperties()->GetInverseMass();
|
||||
}
|
||||
|
||||
Matrix4f RigidBody3D::GetMatrix() const
|
||||
{
|
||||
return FromJolt(m_body->GetCenterOfMassTransform());
|
||||
}
|
||||
|
||||
Vector3f RigidBody3D::GetPosition() const
|
||||
{
|
||||
return FromJolt(m_body->GetPosition());
|
||||
}
|
||||
|
||||
std::pair<Vector3f, Quaternionf> RigidBody3D::GetPositionAndRotation() const
|
||||
{
|
||||
JPH::Vec3 position = m_body->GetPosition();
|
||||
JPH::Quat rotation = m_body->GetRotation();
|
||||
|
||||
return { FromJolt(position), FromJolt(rotation) };
|
||||
}
|
||||
|
||||
Quaternionf RigidBody3D::GetRotation() const
|
||||
{
|
||||
return FromJolt(m_body->GetRotation());
|
||||
}
|
||||
|
||||
bool RigidBody3D::IsSleeping() const
|
||||
{
|
||||
return !m_body->IsActive();
|
||||
}
|
||||
|
||||
bool RigidBody3D::IsSleepingEnabled() const
|
||||
{
|
||||
return m_body->GetAllowSleeping();
|
||||
}
|
||||
|
||||
bool RigidBody3D::IsStatic() const
|
||||
{
|
||||
return m_body->GetMotionType() == JPH::EMotionType::Static;
|
||||
}
|
||||
|
||||
void RigidBody3D::SetAngularDamping(float angularDamping)
|
||||
{
|
||||
if NAZARA_UNLIKELY(IsStatic())
|
||||
return;
|
||||
|
||||
m_body->GetMotionProperties()->SetAngularDamping(angularDamping);
|
||||
}
|
||||
|
||||
void RigidBody3D::SetAngularVelocity(const Vector3f& angularVelocity)
|
||||
{
|
||||
m_body->SetAngularVelocity(ToJolt(angularVelocity));
|
||||
}
|
||||
|
||||
void RigidBody3D::SetGeom(std::shared_ptr<Collider3D> geom, bool recomputeInertia)
|
||||
{
|
||||
if (m_geom != geom)
|
||||
{
|
||||
float mass = GetMass();
|
||||
|
||||
const JPH::Shape* shape;
|
||||
m_geom = std::move(geom);
|
||||
if (m_geom)
|
||||
shape = m_geom->GetShapeSettings()->Create().Get();
|
||||
else
|
||||
{
|
||||
m_body->SetIsSensor(true);
|
||||
shape = m_world->GetNullShape();
|
||||
}
|
||||
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
|
||||
bodyInterface.SetShape(m_body->GetID(), shape, false, (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
|
||||
if (recomputeInertia)
|
||||
{
|
||||
JPH::MassProperties massProperties = m_body->GetShape()->GetMassProperties();
|
||||
massProperties.ScaleToMass(mass);
|
||||
m_body->GetMotionProperties()->SetMassProperties(JPH::EAllowedDOFs::All, massProperties);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RigidBody3D::SetLinearDamping(float damping)
|
||||
{
|
||||
if NAZARA_UNLIKELY(IsStatic())
|
||||
return;
|
||||
|
||||
m_body->GetMotionProperties()->SetLinearDamping(damping);
|
||||
}
|
||||
|
||||
void RigidBody3D::SetLinearVelocity(const Vector3f& velocity)
|
||||
{
|
||||
m_body->SetLinearVelocity(ToJolt(velocity));
|
||||
}
|
||||
|
||||
void RigidBody3D::SetMass(float mass, bool recomputeInertia)
|
||||
{
|
||||
NazaraAssert(mass >= 0.f, "Mass must be positive and finite");
|
||||
NazaraAssert(std::isfinite(mass), "Mass must be positive and finite");
|
||||
|
||||
if (mass > 0.f)
|
||||
{
|
||||
m_body->SetMotionType(JPH::EMotionType::Dynamic);
|
||||
if (recomputeInertia)
|
||||
{
|
||||
JPH::MassProperties massProperties = m_body->GetShape()->GetMassProperties();
|
||||
massProperties.ScaleToMass(mass);
|
||||
m_body->GetMotionProperties()->SetMassProperties(JPH::EAllowedDOFs::All, massProperties);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.DeactivateBody(m_body->GetID());
|
||||
|
||||
m_body->SetMotionType(JPH::EMotionType::Kinematic);
|
||||
}
|
||||
}
|
||||
|
||||
void RigidBody3D::SetPosition(const Vector3f& position)
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetPosition(m_body->GetID(), ToJolt(position), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
|
||||
}
|
||||
|
||||
void RigidBody3D::SetRotation(const Quaternionf& rotation)
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetRotation(m_body->GetID(), ToJolt(rotation), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
|
||||
}
|
||||
|
||||
void RigidBody3D::TeleportTo(const Vector3f& position, const Quaternionf& rotation)
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
|
||||
bodyInterface.SetPositionAndRotation(m_body->GetID(), ToJolt(position), ToJolt(rotation), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
|
||||
}
|
||||
|
||||
Quaternionf RigidBody3D::ToLocal(const Quaternionf& worldRotation)
|
||||
{
|
||||
return GetRotation().Conjugate() * worldRotation;
|
||||
}
|
||||
|
||||
Vector3f RigidBody3D::ToLocal(const Vector3f& worldPosition)
|
||||
{
|
||||
return FromJolt(m_body->GetInverseCenterOfMassTransform() * ToJolt(worldPosition));
|
||||
}
|
||||
|
||||
Quaternionf RigidBody3D::ToWorld(const Quaternionf& localRotation)
|
||||
{
|
||||
return GetRotation() * localRotation;
|
||||
}
|
||||
|
||||
Vector3f RigidBody3D::ToWorld(const Vector3f& localPosition)
|
||||
{
|
||||
return FromJolt(m_body->GetCenterOfMassTransform() * ToJolt(localPosition));
|
||||
}
|
||||
|
||||
void RigidBody3D::WakeUp()
|
||||
{
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
|
||||
bodyInterface.ActivateBody(m_body->GetID());
|
||||
}
|
||||
|
||||
RigidBody3D& RigidBody3D::operator=(RigidBody3D&& body) noexcept
|
||||
{
|
||||
Destroy();
|
||||
|
||||
m_body = std::move(body.m_body);
|
||||
m_bodyIndex = body.m_bodyIndex;
|
||||
m_geom = std::move(body.m_geom);
|
||||
m_world = std::move(body.m_world);
|
||||
m_isSimulationEnabled = body.m_isSimulationEnabled;
|
||||
m_isTrigger = body.m_isTrigger;
|
||||
|
||||
body.m_bodyIndex = std::numeric_limits<UInt32>::max();
|
||||
|
||||
if (m_body)
|
||||
m_body->SetUserData(SafeCast<UInt64>(BitCast<std::uintptr_t>(this)));
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
void RigidBody3D::Create(PhysWorld3D& world, const DynamicSettings& settings)
|
||||
{
|
||||
m_geom = settings.geom;
|
||||
m_isSimulationEnabled = settings.isSimulationEnabled;
|
||||
m_isTrigger = settings.isTrigger;
|
||||
m_world = &world;
|
||||
|
||||
JPH::BodyCreationSettings creationSettings;
|
||||
BuildSettings(settings, creationSettings);
|
||||
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
|
||||
m_body = bodyInterface.CreateBody(creationSettings);
|
||||
|
||||
JPH::BodyID bodyId = m_body->GetID();
|
||||
m_bodyIndex = bodyId.GetIndex();
|
||||
|
||||
if (settings.isSimulationEnabled)
|
||||
m_world->RegisterBody(bodyId, !settings.initiallySleeping, false);
|
||||
}
|
||||
|
||||
void RigidBody3D::Create(PhysWorld3D& world, const StaticSettings& settings)
|
||||
{
|
||||
m_geom = settings.geom;
|
||||
m_isSimulationEnabled = settings.isSimulationEnabled;
|
||||
m_isTrigger = settings.isTrigger;
|
||||
m_world = &world;
|
||||
|
||||
JPH::BodyCreationSettings creationSettings;
|
||||
BuildSettings(settings, creationSettings);
|
||||
|
||||
JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
|
||||
m_body = bodyInterface.CreateBody(creationSettings);
|
||||
|
||||
JPH::BodyID bodyId = m_body->GetID();
|
||||
m_bodyIndex = bodyId.GetIndex();
|
||||
|
||||
if (settings.isSimulationEnabled)
|
||||
m_world->RegisterBody(bodyId, false, false); //< static bodies cannot be activated
|
||||
}
|
||||
|
||||
void RigidBody3D::Destroy(bool worldDestruction)
|
||||
{
|
||||
if (m_body)
|
||||
{
|
||||
m_world->UnregisterBody(m_body->GetID(), true, !worldDestruction);
|
||||
m_body = nullptr;
|
||||
}
|
||||
|
||||
m_geom.reset();
|
||||
}
|
||||
|
||||
void RigidBody3D::BuildSettings(const DynamicSettings& settings, JPH::BodyCreationSettings& creationSettings)
|
||||
{
|
||||
BuildSettings(static_cast<const CommonSettings&>(settings), creationSettings);
|
||||
|
||||
creationSettings.mAngularDamping = settings.angularDamping;
|
||||
creationSettings.mAngularVelocity = ToJolt(settings.angularVelocity);
|
||||
creationSettings.mLinearDamping = settings.linearDamping;
|
||||
creationSettings.mLinearVelocity = ToJolt(settings.linearVelocity);
|
||||
creationSettings.mFriction = settings.friction;
|
||||
creationSettings.mGravityFactor = settings.gravityFactor;
|
||||
creationSettings.mMaxAngularVelocity = settings.maxAngularVelocity;
|
||||
creationSettings.mMaxLinearVelocity = settings.maxLinearVelocity;
|
||||
creationSettings.mObjectLayer = 1;
|
||||
creationSettings.mRestitution = settings.restitution;
|
||||
|
||||
creationSettings.mMotionType = (settings.mass > 0.f) ? JPH::EMotionType::Dynamic : JPH::EMotionType::Kinematic;
|
||||
|
||||
float mass = settings.mass;
|
||||
if (mass <= 0.f)
|
||||
mass = 1.f;
|
||||
|
||||
creationSettings.mMassPropertiesOverride = creationSettings.GetShape()->GetMassProperties();
|
||||
creationSettings.mMassPropertiesOverride.ScaleToMass(mass);
|
||||
creationSettings.mOverrideMassProperties = JPH::EOverrideMassProperties::MassAndInertiaProvided;
|
||||
|
||||
|
||||
switch (settings.motionQuality)
|
||||
{
|
||||
case PhysMotionQuality3D::Discrete: creationSettings.mMotionQuality = JPH::EMotionQuality::Discrete; break;
|
||||
case PhysMotionQuality3D::LinearCast: creationSettings.mMotionQuality = JPH::EMotionQuality::LinearCast; break;
|
||||
}
|
||||
}
|
||||
|
||||
void RigidBody3D::BuildSettings(const StaticSettings& settings, JPH::BodyCreationSettings& creationSettings)
|
||||
{
|
||||
BuildSettings(static_cast<const CommonSettings&>(settings), creationSettings);
|
||||
|
||||
creationSettings.mMotionType = JPH::EMotionType::Static;
|
||||
}
|
||||
|
||||
void RigidBody3D::BuildSettings(const CommonSettings& settings, JPH::BodyCreationSettings& creationSettings)
|
||||
{
|
||||
if (settings.geom)
|
||||
{
|
||||
creationSettings.SetShapeSettings(settings.geom->GetShapeSettings());
|
||||
creationSettings.mIsSensor = settings.isTrigger;
|
||||
}
|
||||
else
|
||||
{
|
||||
creationSettings.SetShape(m_world->GetNullShape());
|
||||
creationSettings.mIsSensor = true; //< not the best solution but enough for now
|
||||
}
|
||||
|
||||
creationSettings.mPosition = ToJolt(settings.position);
|
||||
creationSettings.mRotation = ToJolt(settings.rotation);
|
||||
creationSettings.mUserData = SafeCast<UInt64>(BitCast<std::uintptr_t>(this));
|
||||
}
|
||||
|
||||
bool RigidBody3D::ShouldActivate() const
|
||||
{
|
||||
return m_isSimulationEnabled && m_world->IsBodyRegistered(m_bodyIndex);
|
||||
}
|
||||
}
|
||||
216
src/Nazara/Physics3D/Systems/Physics3DSystem.cpp
Normal file
216
src/Nazara/Physics3D/Systems/Physics3DSystem.cpp
Normal file
@@ -0,0 +1,216 @@
|
||||
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics3D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/Physics3D/Systems/Physics3DSystem.hpp>
|
||||
#include <Nazara/Core/Components/DisabledComponent.hpp>
|
||||
#include <Nazara/Physics3D/Physics3DBody.hpp>
|
||||
#include <Nazara/Utility/Components/NodeComponent.hpp>
|
||||
#include <Nazara/Physics3D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
Physics3DSystem::Physics3DSystem(entt::registry& registry) :
|
||||
m_registry(registry),
|
||||
m_characterConstructObserver(m_registry, entt::collector.group<PhysCharacter3DComponent, NodeComponent>(entt::exclude<DisabledComponent, RigidBody3DComponent>)),
|
||||
m_rigidBodyConstructObserver(m_registry, entt::collector.group<RigidBody3DComponent, NodeComponent>(entt::exclude<DisabledComponent, PhysCharacter3DComponent>))
|
||||
{
|
||||
m_bodyConstructConnection = registry.on_construct<RigidBody3DComponent>().connect<&Physics3DSystem::OnBodyConstruct>(this);
|
||||
m_bodyDestructConnection = registry.on_destroy<RigidBody3DComponent>().connect<&Physics3DSystem::OnBodyDestruct>(this);
|
||||
m_characterConstructConnection = registry.on_construct<PhysCharacter3DComponent>().connect<&Physics3DSystem::OnCharacterConstruct>(this);
|
||||
m_characterDestructConnection = registry.on_destroy<PhysCharacter3DComponent>().connect<&Physics3DSystem::OnCharacterDestruct>(this);
|
||||
}
|
||||
|
||||
Physics3DSystem::~Physics3DSystem()
|
||||
{
|
||||
m_characterConstructObserver.disconnect();
|
||||
m_rigidBodyConstructObserver.disconnect();
|
||||
|
||||
// Ensure every RigidBody3D is destroyed before world is
|
||||
auto characterView = m_registry.view<PhysCharacter3DComponent>();
|
||||
for (auto [entity, characterComponent] : characterView.each())
|
||||
characterComponent.Destroy();
|
||||
|
||||
auto rigidBodyView = m_registry.view<RigidBody3DComponent>();
|
||||
for (auto [entity, rigidBodyComponent] : rigidBodyView.each())
|
||||
rigidBodyComponent.Destroy(true);
|
||||
}
|
||||
|
||||
bool Physics3DSystem::CollisionQuery(const Vector3f& point, const FunctionRef<std::optional<float>(const PointCollisionInfo& collisionInfo)>& callback)
|
||||
{
|
||||
return m_physWorld.CollisionQuery(point, [&](const PhysWorld3D::PointCollisionInfo& hitInfo)
|
||||
{
|
||||
PointCollisionInfo extendedHitInfo;
|
||||
static_cast<PhysWorld3D::PointCollisionInfo&>(extendedHitInfo) = hitInfo;
|
||||
|
||||
if (extendedHitInfo.hitBody)
|
||||
{
|
||||
std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex();
|
||||
if (bodyIndex < m_bodyIndicesToEntity.size())
|
||||
extendedHitInfo.hitEntity = entt::handle(m_registry, m_bodyIndicesToEntity[bodyIndex]);
|
||||
}
|
||||
|
||||
return callback(extendedHitInfo);
|
||||
});
|
||||
}
|
||||
|
||||
bool Physics3DSystem::CollisionQuery(const Collider3D& collider, const Matrix4f& shapeTransform, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
|
||||
{
|
||||
return CollisionQuery(collider, shapeTransform, Vector3f::Unit(), callback);
|
||||
}
|
||||
|
||||
bool Physics3DSystem::CollisionQuery(const Collider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
|
||||
{
|
||||
return m_physWorld.CollisionQuery(collider, colliderTransform, colliderScale, [&](const PhysWorld3D::ShapeCollisionInfo& hitInfo)
|
||||
{
|
||||
ShapeCollisionInfo extendedHitInfo;
|
||||
static_cast<PhysWorld3D::ShapeCollisionInfo&>(extendedHitInfo) = hitInfo;
|
||||
|
||||
if (extendedHitInfo.hitBody)
|
||||
{
|
||||
std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex();
|
||||
if (bodyIndex < m_bodyIndicesToEntity.size())
|
||||
extendedHitInfo.hitEntity = entt::handle(m_registry, m_bodyIndicesToEntity[bodyIndex]);
|
||||
}
|
||||
|
||||
return callback(extendedHitInfo);
|
||||
});
|
||||
}
|
||||
|
||||
bool Physics3DSystem::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef<std::optional<float>(const RaycastHit& hitInfo)>& callback)
|
||||
{
|
||||
return m_physWorld.RaycastQuery(from, to, [&](const PhysWorld3D::RaycastHit& hitInfo)
|
||||
{
|
||||
RaycastHit extendedHitInfo;
|
||||
static_cast<PhysWorld3D::RaycastHit&>(extendedHitInfo) = hitInfo;
|
||||
|
||||
if (extendedHitInfo.hitBody)
|
||||
{
|
||||
std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex();
|
||||
if (bodyIndex < m_bodyIndicesToEntity.size())
|
||||
extendedHitInfo.hitEntity = entt::handle(m_registry, m_bodyIndicesToEntity[bodyIndex]);
|
||||
}
|
||||
|
||||
return callback(extendedHitInfo);
|
||||
});
|
||||
}
|
||||
|
||||
bool Physics3DSystem::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef<void(const RaycastHit& hitInfo)>& callback)
|
||||
{
|
||||
return m_physWorld.RaycastQueryFirst(from, to, [&](const PhysWorld3D::RaycastHit& hitInfo)
|
||||
{
|
||||
RaycastHit extendedHitInfo;
|
||||
static_cast<PhysWorld3D::RaycastHit&>(extendedHitInfo) = hitInfo;
|
||||
|
||||
if (extendedHitInfo.hitBody)
|
||||
{
|
||||
std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex();
|
||||
if (bodyIndex < m_bodyIndicesToEntity.size())
|
||||
extendedHitInfo.hitEntity = entt::handle(m_registry, m_bodyIndicesToEntity[bodyIndex]);
|
||||
}
|
||||
|
||||
callback(extendedHitInfo);
|
||||
});
|
||||
}
|
||||
|
||||
void Physics3DSystem::Update(Time elapsedTime)
|
||||
{
|
||||
// Move newly-created physics entities to their node position/rotation
|
||||
m_characterConstructObserver.each([this](entt::entity entity)
|
||||
{
|
||||
PhysCharacter3DComponent& entityCharacter = m_registry.get<PhysCharacter3DComponent>(entity);
|
||||
NodeComponent& entityNode = m_registry.get<NodeComponent>(entity);
|
||||
|
||||
entityCharacter.TeleportTo(entityNode.GetPosition(), entityNode.GetRotation());
|
||||
});
|
||||
|
||||
m_rigidBodyConstructObserver.each([this](entt::entity entity)
|
||||
{
|
||||
RigidBody3DComponent& entityBody = m_registry.get<RigidBody3DComponent>(entity);
|
||||
NodeComponent& entityNode = m_registry.get<NodeComponent>(entity);
|
||||
|
||||
entityBody.TeleportTo(entityNode.GetPosition(), entityNode.GetRotation());
|
||||
});
|
||||
|
||||
// Update the physics world
|
||||
if (!m_physWorld.Step(elapsedTime))
|
||||
return; // No physics step took place
|
||||
|
||||
// Replicate characters to their NodeComponent
|
||||
{
|
||||
auto view = m_registry.view<NodeComponent, const PhysCharacter3DComponent>(entt::exclude<DisabledComponent>);
|
||||
for (auto entity : view)
|
||||
{
|
||||
auto& characterComponent = view.get<const PhysCharacter3DComponent>(entity);
|
||||
if (!m_physWorld.IsBodyActive(characterComponent.GetBodyIndex()))
|
||||
continue;
|
||||
|
||||
auto& nodeComponent = view.get<NodeComponent>(entity);
|
||||
|
||||
auto [position, rotation] = characterComponent.GetPositionAndRotation();
|
||||
nodeComponent.SetTransform(position, rotation);
|
||||
}
|
||||
}
|
||||
|
||||
// Replicate active rigid body position to their node components
|
||||
{
|
||||
auto view = m_registry.view<NodeComponent, const RigidBody3DComponent>(entt::exclude<DisabledComponent>);
|
||||
for (auto entity : m_registry.view<NodeComponent, const RigidBody3DComponent>(entt::exclude<DisabledComponent>))
|
||||
{
|
||||
auto& rigidBodyComponent = view.get<const RigidBody3DComponent>(entity);
|
||||
if (!m_physWorld.IsBodyActive(rigidBodyComponent.GetBodyIndex()))
|
||||
continue;
|
||||
|
||||
auto& nodeComponent = view.get<NodeComponent>(entity);
|
||||
|
||||
auto [position, rotation] = rigidBodyComponent.GetPositionAndRotation();
|
||||
nodeComponent.SetTransform(position, rotation);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Physics3DSystem::OnBodyConstruct(entt::registry& registry, entt::entity entity)
|
||||
{
|
||||
// Register rigid body owning entity
|
||||
RigidBody3DComponent& rigidBody = registry.get<RigidBody3DComponent>(entity);
|
||||
rigidBody.Construct(m_physWorld);
|
||||
|
||||
UInt32 uniqueIndex = rigidBody.GetBodyIndex();
|
||||
if (uniqueIndex >= m_bodyIndicesToEntity.size())
|
||||
m_bodyIndicesToEntity.resize(uniqueIndex + 1);
|
||||
|
||||
m_bodyIndicesToEntity[uniqueIndex] = entity;
|
||||
}
|
||||
|
||||
void Physics3DSystem::OnBodyDestruct(entt::registry& registry, entt::entity entity)
|
||||
{
|
||||
// Unregister owning entity
|
||||
RigidBody3DComponent& rigidBody = registry.get<RigidBody3DComponent>(entity);
|
||||
UInt32 uniqueIndex = rigidBody.GetBodyIndex();
|
||||
assert(uniqueIndex <= m_bodyIndicesToEntity.size());
|
||||
|
||||
m_bodyIndicesToEntity[uniqueIndex] = entt::null;
|
||||
}
|
||||
|
||||
void Physics3DSystem::OnCharacterConstruct(entt::registry& registry, entt::entity entity)
|
||||
{
|
||||
PhysCharacter3DComponent& character = registry.get<PhysCharacter3DComponent>(entity);
|
||||
character.Construct(m_physWorld);
|
||||
|
||||
UInt32 uniqueIndex = character.GetBodyIndex();
|
||||
if (uniqueIndex >= m_bodyIndicesToEntity.size())
|
||||
m_bodyIndicesToEntity.resize(uniqueIndex + 1);
|
||||
|
||||
m_bodyIndicesToEntity[uniqueIndex] = entity;
|
||||
}
|
||||
|
||||
void Physics3DSystem::OnCharacterDestruct(entt::registry& registry, entt::entity entity)
|
||||
{
|
||||
// Unregister owning entity
|
||||
PhysCharacter3DComponent& character = registry.get<PhysCharacter3DComponent>(entity);
|
||||
UInt32 uniqueIndex = character.GetBodyIndex();
|
||||
assert(uniqueIndex <= m_bodyIndicesToEntity.size());
|
||||
|
||||
m_bodyIndicesToEntity[uniqueIndex] = entt::null;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user