464 lines
16 KiB
C++
464 lines
16 KiB
C++
// Copyright (C) 2024 Jérôme "SirLynix" Leclercq (lynix680@gmail.com)
|
|
// This file is part of the "Nazara Engine - JoltPhysics3D module"
|
|
// For conditions of distribution and use, see copyright notice in Config.hpp
|
|
|
|
#include <Nazara/JoltPhysics3D/JoltCollider3D.hpp>
|
|
#include <Nazara/Core/PrimitiveList.hpp>
|
|
#include <Nazara/JoltPhysics3D/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/JoltPhysics3D/Debug.hpp>
|
|
|
|
namespace Nz
|
|
{
|
|
JoltCollider3D::JoltCollider3D() = default;
|
|
JoltCollider3D::~JoltCollider3D() = default;
|
|
|
|
std::shared_ptr<StaticMesh> JoltCollider3D::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<JoltCollider3D> JoltCollider3D::Build(const PrimitiveList& list)
|
|
{
|
|
std::size_t primitiveCount = list.GetSize();
|
|
if (primitiveCount > 1)
|
|
{
|
|
std::vector<JoltCompoundCollider3D::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<JoltCompoundCollider3D>(std::move(childColliders));
|
|
}
|
|
else if (primitiveCount > 0)
|
|
return CreateGeomFromPrimitive(list.GetPrimitive(0));
|
|
else
|
|
return nullptr;// std::make_shared<NullCollider3D>(); //< TODO
|
|
}
|
|
|
|
void JoltCollider3D::ResetShapeSettings()
|
|
{
|
|
m_shapeSettings.reset();
|
|
}
|
|
|
|
void JoltCollider3D::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<JoltCollider3D> JoltCollider3D::CreateGeomFromPrimitive(const Primitive& primitive)
|
|
{
|
|
switch (primitive.type)
|
|
{
|
|
case PrimitiveType::Box:
|
|
return std::make_shared<JoltBoxCollider3D>(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<JoltBoxCollider3D>(Vector3f(primitive.plane.size.x, 0.01f, primitive.plane.size.y));
|
|
///TODO: PlaneGeom?
|
|
|
|
case PrimitiveType::Sphere:
|
|
return std::make_shared<JoltSphereCollider3D>(primitive.sphere.size);
|
|
}
|
|
|
|
NazaraErrorFmt("Primitive type not handled ({0:#x})", UnderlyingCast(primitive.type));
|
|
return std::shared_ptr<JoltCollider3D>();
|
|
}
|
|
|
|
/********************************** JoltBoxCollider3D **********************************/
|
|
|
|
JoltBoxCollider3D::JoltBoxCollider3D(const Vector3f& lengths, float convexRadius)
|
|
{
|
|
SetupShapeSettings(std::make_unique<JPH::BoxShapeSettings>(ToJolt(lengths * 0.5f), convexRadius));
|
|
}
|
|
|
|
void JoltBoxCollider3D::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 JoltBoxCollider3D::GetLengths() const
|
|
{
|
|
return FromJolt(GetShapeSettingsAs<JPH::BoxShapeSettings>()->mHalfExtent) * 2.f;
|
|
}
|
|
|
|
JoltColliderType3D JoltBoxCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::Box;
|
|
}
|
|
|
|
/********************************** JoltCapsuleCollider3D **********************************/
|
|
|
|
JoltCapsuleCollider3D::JoltCapsuleCollider3D(float height, float radius)
|
|
{
|
|
SetupShapeSettings(std::make_unique<JPH::CapsuleShapeSettings>(height * 0.5f, radius));
|
|
}
|
|
|
|
void JoltCapsuleCollider3D::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 JoltCapsuleCollider3D::GetHeight() const
|
|
{
|
|
return GetShapeSettingsAs<JPH::CapsuleShapeSettings>()->mHalfHeightOfCylinder * 2.0f;
|
|
}
|
|
|
|
float JoltCapsuleCollider3D::GetRadius() const
|
|
{
|
|
return GetShapeSettingsAs<JPH::CapsuleShapeSettings>()->mRadius;
|
|
}
|
|
|
|
JoltColliderType3D JoltCapsuleCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::Capsule;
|
|
}
|
|
|
|
/******************************* JoltCompoundCollider3D ********************************/
|
|
|
|
JoltCompoundCollider3D::JoltCompoundCollider3D(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));
|
|
}
|
|
|
|
JoltCompoundCollider3D::~JoltCompoundCollider3D()
|
|
{
|
|
// We have to destroy shape settings first as it carries references on the inner colliders
|
|
ResetShapeSettings();
|
|
}
|
|
|
|
void JoltCompoundCollider3D::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 JoltCompoundCollider3D::GetGeoms() const -> const std::vector<ChildCollider>&
|
|
{
|
|
return m_childs;
|
|
}
|
|
|
|
JoltColliderType3D JoltCompoundCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::Compound;
|
|
}
|
|
|
|
/******************************** JoltConvexHullCollider3D *********************************/
|
|
|
|
JoltConvexHullCollider3D::JoltConvexHullCollider3D(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 JoltConvexHullCollider3D::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]));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
JoltColliderType3D JoltConvexHullCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::Convex;
|
|
}
|
|
|
|
/******************************** JoltMeshCollider3D *********************************/
|
|
|
|
JoltMeshCollider3D::JoltMeshCollider3D(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));
|
|
}
|
|
|
|
JoltMeshCollider3D::JoltMeshCollider3D(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 JoltMeshCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
|
{
|
|
}
|
|
|
|
JoltColliderType3D JoltMeshCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::Mesh;
|
|
}
|
|
|
|
/******************************** JoltSphereCollider3D *********************************/
|
|
|
|
JoltSphereCollider3D::JoltSphereCollider3D(float radius)
|
|
{
|
|
SetupShapeSettings(std::make_unique<JPH::SphereShapeSettings>(radius));
|
|
}
|
|
|
|
void JoltSphereCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
|
|
{
|
|
}
|
|
|
|
float JoltSphereCollider3D::GetRadius() const
|
|
{
|
|
return GetShapeSettingsAs<JPH::SphereShapeSettings>()->mRadius;
|
|
}
|
|
|
|
JoltColliderType3D JoltSphereCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::Sphere;
|
|
}
|
|
|
|
/******************************** JoltTranslatedRotatedCollider3D *********************************/
|
|
|
|
JoltTranslatedRotatedCollider3D::JoltTranslatedRotatedCollider3D(std::shared_ptr<JoltCollider3D> 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()));
|
|
}
|
|
|
|
JoltTranslatedRotatedCollider3D::~JoltTranslatedRotatedCollider3D()
|
|
{
|
|
// We have to destroy shape settings first as it carries references on the inner collider
|
|
ResetShapeSettings();
|
|
}
|
|
|
|
void JoltTranslatedRotatedCollider3D::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)));
|
|
}
|
|
|
|
JoltColliderType3D JoltTranslatedRotatedCollider3D::GetType() const
|
|
{
|
|
return JoltColliderType3D::TranslatedRotatedDecoration;
|
|
}
|
|
}
|