Implementing basic physics
This commit is contained in:
Dynamitos
+4
-1
@@ -39,4 +39,7 @@
|
||||
url = https://github.com/madler/zlib.git
|
||||
[submodule "external/CRCpp"]
|
||||
path = external/CRCpp
|
||||
url = https://github.com/d-bahr/CRCpp.git
|
||||
url = https://github.com/d-bahr/CRCpp.git
|
||||
[submodule "external/odeint"]
|
||||
path = external/odeint
|
||||
url = https://github.com/headmyshoulder/odeint-v2.git
|
||||
+18
-7
@@ -10,6 +10,7 @@ option (USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON)
|
||||
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
set(CMAKE_COMPILE_WARNING_AS_ERROR OFF)
|
||||
set(CMAKE_INSTALL_MESSAGE LAZY)
|
||||
set(BUILD_SHARED_LIBS OFF)
|
||||
|
||||
set(ENGINE_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/src/Engine)
|
||||
@@ -27,7 +28,8 @@ set(TTFPARSER_ROOT ${EXTERNAL_ROOT}/ttf-parser)
|
||||
set(FREETYPE_ROOT ${EXTERNAL_ROOT}/freetype)
|
||||
set(SPIRV_ROOT ${EXTERNAL_ROOT}/SPIRV-Cross)
|
||||
set(ENTT_ROOT ${EXTERNAL_ROOT}/entt)
|
||||
set(NSAM_ROOT ${EXTERNAL_ROOT}/aftermath)
|
||||
set(NLOPT_ROOT ${EXTERNAL_ROOT}/nlopt)
|
||||
set(ODEINT_ROOT ${EXTERNAL_ROOT}/odeint)
|
||||
set(THREADPOOL_ROOT ${EXTERNAL_ROOT}/thread-pool)
|
||||
set(ZLIB_ROOT ${EXTERNAL_ROOT}/zlib)
|
||||
|
||||
@@ -52,7 +54,6 @@ if(WIN32)
|
||||
endif()
|
||||
|
||||
find_package(Vulkan REQUIRED)
|
||||
find_package(Boost REQUIRED serialization)
|
||||
|
||||
if(WIN32)
|
||||
add_compile_definitions(USE_EXTENSIONS)
|
||||
@@ -77,6 +78,7 @@ target_link_libraries(Engine PUBLIC stb)
|
||||
target_link_libraries(Engine PUBLIC nlohmann_json::nlohmann_json)
|
||||
target_link_libraries(Engine PUBLIC dp::thread-pool)
|
||||
target_link_libraries(Engine PUBLIC crcpp)
|
||||
target_link_libraries(Engine PUBLIC odeint)
|
||||
if(UNIX)
|
||||
target_link_libraries(Engine PUBLIC Threads::Threads)
|
||||
target_link_libraries(Engine PUBLIC dl)
|
||||
@@ -101,19 +103,24 @@ target_precompile_headers(Engine
|
||||
if(MSVC)
|
||||
set(_CRT_SECURE_NO_WARNINGS)
|
||||
target_compile_options(Engine PUBLIC /Zi /MP14 /W4 /DEBUG "/WX-")
|
||||
target_sources(Engine INTERFACE
|
||||
$<BUILD_INTERFACE:Seele.natvis>
|
||||
$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/Seele.natvis>
|
||||
)
|
||||
install(FILES
|
||||
Seele.natvis
|
||||
DESTINATION .)
|
||||
else()
|
||||
target_compile_options(Engine PUBLIC -g -Wall -Wextra -Wno-delete-incomplete -pedantic -fcoroutines)
|
||||
endif()
|
||||
|
||||
add_subdirectory(src/)
|
||||
|
||||
add_executable(Seele_unit_tests "")
|
||||
add_subdirectory(test/)
|
||||
#add_executable(Seele_unit_tests "")
|
||||
#add_subdirectory(test/)
|
||||
|
||||
add_custom_target(SeeleEngine ALL
|
||||
COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_SOURCE_DIR}/res $<TARGET_FILE_DIR:Engine>
|
||||
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_PROPERTY:slang,SLANG_BINARY> $<TARGET_FILE_DIR:Engine>
|
||||
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_PROPERTY:slang,GLSLANG_BINARY> $<TARGET_FILE_DIR:Engine>
|
||||
COMMAND_EXPAND_LISTS
|
||||
DEPENDS Editor slang-build)
|
||||
|
||||
@@ -124,7 +131,6 @@ install(
|
||||
glfw
|
||||
glm
|
||||
freetype
|
||||
slang
|
||||
assimp
|
||||
ktx
|
||||
stb
|
||||
@@ -132,7 +138,12 @@ install(
|
||||
nlohmann_json
|
||||
ThreadPool
|
||||
zlib
|
||||
slang
|
||||
slang-glslang
|
||||
slang-llvm
|
||||
zlibstatic
|
||||
IrrXML
|
||||
odeint
|
||||
EXPORT
|
||||
EngineTargets
|
||||
FILE_SET HEADERS
|
||||
|
||||
+54
-4
@@ -10,6 +10,7 @@ add_subdirectory(${FREETYPE_ROOT})
|
||||
set(ASSIMP_BUILD_TESTS OFF CACHE INTERNAL "")
|
||||
set(ASSIMP_BUILD_SAMPLES OFF CACHE INTERNAL "")
|
||||
set(ASSIMP_BUILD_OVERALLS OFF CACHE INTERNAL "")
|
||||
set(ASSIMP_BUILD_ASSIMP_TOOLS OFF CACHE INTERNAL "")
|
||||
add_subdirectory(${ASSIMP_ROOT})
|
||||
target_compile_definitions(assimp PRIVATE _SILENCE_CXX17_ITERATOR_BASE_CLASS_DEPRECATION_WARNING)
|
||||
if(WIN32)
|
||||
@@ -21,6 +22,7 @@ endif()
|
||||
|
||||
#-----------------KTX----------------------------
|
||||
set(KTX_FEATURE_TESTS off)
|
||||
set(KTX_FEATURE_TOOLS off)
|
||||
|
||||
add_subdirectory(${KTX_ROOT} ${KTX_ROOT})
|
||||
|
||||
@@ -71,15 +73,36 @@ ExternalProject_Add(slang-build
|
||||
)
|
||||
endif()
|
||||
|
||||
add_library(slang-llvm INTERFACE)
|
||||
target_link_libraries(slang-llvm INTERFACE
|
||||
$<BUILD_INTERFACE:${SLANG_BINARY_DIR}/slang.lib>
|
||||
$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/lib/slang.lib>
|
||||
)
|
||||
set_target_properties(slang-llvm PROPERTIES IMPORTED_IMPLIB ${SLANG_BINARY_DIR}/slang.lib)
|
||||
set_target_properties(slang-llvm PROPERTIES IMPORTED_LOCATION ${SLANG_BINARY_DIR}/slang-llvm.dll)
|
||||
|
||||
add_library(slang-glslang INTERFACE)
|
||||
target_link_libraries(slang-glslang INTERFACE
|
||||
$<BUILD_INTERFACE:${SLANG_BINARY_DIR}/slang.lib>
|
||||
$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/lib/slang.lib>
|
||||
)
|
||||
set_target_properties(slang-glslang PROPERTIES IMPORTED_IMPLIB ${SLANG_BINARY_DIR}/slang.lib)
|
||||
set_target_properties(slang-glslang PROPERTIES IMPORTED_LOCATION ${SLANG_BINARY_DIR}/slang-glslang.dll)
|
||||
|
||||
add_library(slang INTERFACE)
|
||||
|
||||
target_include_directories(slang INTERFACE
|
||||
$<BUILD_INTERFACE:${SLANG_ROOT}/>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_link_libraries(slang INTERFACE ${SLANG_BINARY_DIR}/*.lib)
|
||||
set_target_properties(slang PROPERTIES SLANG_BINARY ${SLANG_BINARY_DIR}/slang.dll)
|
||||
set_target_properties(slang PROPERTIES GLSLANG_BINARY ${SLANG_BINARY_DIR}/slang-glslang.dll)
|
||||
target_link_libraries(slang INTERFACE
|
||||
$<BUILD_INTERFACE:${SLANG_BINARY_DIR}/slang.lib>
|
||||
$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/lib/slang.lib>
|
||||
)
|
||||
target_link_libraries(slang INTERFACE slang-glslang)
|
||||
target_link_libraries(slang INTERFACE slang-llvm)
|
||||
set_target_properties(slang PROPERTIES IMPORTED_IMPLIB ${SLANG_BINARY_DIR}/slang.lib)
|
||||
set_target_properties(slang PROPERTIES IMPORTED_LOCATION ${SLANG_BINARY_DIR}/slang.dll)
|
||||
|
||||
install(DIRECTORY
|
||||
${SLANG_ROOT}
|
||||
@@ -87,6 +110,20 @@ install(DIRECTORY
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
install(FILES
|
||||
$<TARGET_PROPERTY:slang,IMPORTED_IMPLIB>
|
||||
$<TARGET_PROPERTY:slang-glslang,IMPORTED_IMPLIB>
|
||||
$<TARGET_PROPERTY:slang-llvm,IMPORTED_IMPLIB>
|
||||
DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
|
||||
)
|
||||
|
||||
install(FILES
|
||||
$<TARGET_PROPERTY:slang,IMPORTED_LOCATION>
|
||||
$<TARGET_PROPERTY:slang-glslang,IMPORTED_LOCATION>
|
||||
$<TARGET_PROPERTY:slang-llvm,IMPORTED_LOCATION>
|
||||
DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
|
||||
)
|
||||
|
||||
#--------------CRC++------------------------------
|
||||
add_library(crcpp INTERFACE)
|
||||
|
||||
@@ -128,8 +165,21 @@ target_include_directories(stb INTERFACE
|
||||
)
|
||||
|
||||
install(DIRECTORY
|
||||
${STB_HEADERS}
|
||||
${STB_ROOT}
|
||||
DESTINATION ${CMAKE_INSTALL_PREFIX}/include
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
#--------------ODEINT-----------------------------------
|
||||
add_library(odeint INTERFACE)
|
||||
|
||||
target_include_directories(odeint INTERFACE
|
||||
$<BUILD_INTERFACE:${ODEINT_ROOT}>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
install(DIRECTORY
|
||||
${ODEINT_ROOT}
|
||||
DESTINATION ${CMAKE_INSTALL_PREFIX}/include
|
||||
FILES_MATCHING PATTERN "*.hpp"
|
||||
)
|
||||
|
||||
+1
Submodule external/odeint added at 2bfbe61a00
@@ -17,6 +17,7 @@ add_subdirectory(Containers/)
|
||||
add_subdirectory(Graphics/)
|
||||
add_subdirectory(Material/)
|
||||
add_subdirectory(Math/)
|
||||
add_subdirectory(Physics/)
|
||||
add_subdirectory(Scene/)
|
||||
add_subdirectory(System/)
|
||||
add_subdirectory(UI/)
|
||||
|
||||
@@ -0,0 +1,105 @@
|
||||
#pragma once
|
||||
#include "Math/Vector.h"
|
||||
#include "Math/Matrix.h"
|
||||
#include "Containers/Array.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
struct AABB
|
||||
{
|
||||
Math::Vector min = Math::Vector(std::numeric_limits<float>::max());
|
||||
Math::Vector max = Math::Vector(std::numeric_limits<float>::min());
|
||||
float surfaceArea() const
|
||||
{
|
||||
Math::Vector d = max - min;
|
||||
return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
|
||||
}
|
||||
bool intersects(const AABB& other) const
|
||||
{
|
||||
if (min.x > other.max.x
|
||||
|| max.x < other.min.x)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (min.y > other.max.y
|
||||
|| max.y < other.min.y)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (min.z > other.max.z
|
||||
|| max.z < other.min.z)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool contains(const AABB& other) const
|
||||
{
|
||||
if (min.x > other.min.x
|
||||
|| max.x < other.max.x)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (min.y > other.min.y
|
||||
|| max.y < other.max.y)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (min.z > other.min.z
|
||||
|| max.z < other.max.z)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
AABB getTransformedBox(const Math::Matrix4& matrix) const
|
||||
{
|
||||
StaticArray<Math::Vector, 8> corners;
|
||||
corners[0] = Math::Vector(min.x, min.y, min.z);
|
||||
corners[1] = Math::Vector(min.x, min.y, max.z);
|
||||
corners[2] = Math::Vector(min.x, max.y, min.z);
|
||||
corners[3] = Math::Vector(min.x, max.y, max.z);
|
||||
corners[4] = Math::Vector(max.x, min.y, min.z);
|
||||
corners[5] = Math::Vector(max.x, min.y, max.z);
|
||||
corners[6] = Math::Vector(max.x, max.y, min.z);
|
||||
corners[7] = Math::Vector(max.x, max.y, max.z);
|
||||
Math::Vector tmin = Math::Vector(1, 1, 1) * std::numeric_limits<float>::max();
|
||||
Math::Vector tmax = Math::Vector(1, 1, 1) * std::numeric_limits<float>::min();
|
||||
for(int i = 0; i < 8; ++i)
|
||||
{
|
||||
Math::Vector transformed = matrix * Math::Vector4(corners[i], 1.0f);
|
||||
tmin = Math::Vector(std::min(tmin.x, transformed.x), std::min(tmin.y, transformed.y), std::min(tmin.z, transformed.z));
|
||||
tmax = Math::Vector(std::max(tmax.x, transformed.x), std::max(tmax.y, transformed.y), std::max(tmax.z, transformed.z));
|
||||
}
|
||||
return AABB {
|
||||
.min = tmin,
|
||||
.max = tmax,
|
||||
};
|
||||
}
|
||||
void adjust(const Math::Vector vertex)
|
||||
{
|
||||
min.x = std::min(min.x, vertex.x);
|
||||
min.y = std::min(min.y, vertex.y);
|
||||
min.z = std::min(min.z, vertex.z);
|
||||
|
||||
max.x = std::max(max.x, vertex.x);
|
||||
max.y = std::max(max.y, vertex.y);
|
||||
max.z = std::max(max.z, vertex.z);
|
||||
}
|
||||
AABB combine(const AABB& other) const
|
||||
{
|
||||
return AABB {
|
||||
.min = Math::Vector(
|
||||
std::min(min.x, other.min.x),
|
||||
std::min(min.y, other.min.y),
|
||||
std::min(min.z, other.min.z)
|
||||
),
|
||||
.max = Math::Vector (
|
||||
std::max(max.x, other.max.x),
|
||||
std::max(max.y, other.max.y),
|
||||
std::max(max.z, other.max.z)
|
||||
),
|
||||
};
|
||||
}
|
||||
};
|
||||
} // namespace Seele
|
||||
@@ -1,8 +1,17 @@
|
||||
target_sources(Engine
|
||||
PRIVATE
|
||||
AABB.h
|
||||
BoxCollider.h
|
||||
Camera.h
|
||||
Camera.cpp
|
||||
Component.h
|
||||
Collider.h
|
||||
Collider.cpp
|
||||
MeshCollider.h
|
||||
RigidBody.h
|
||||
ShapeBase.h
|
||||
ShapeBase.cpp
|
||||
SphereCollider.h
|
||||
StaticMesh.h
|
||||
Transform.h
|
||||
Transform.cpp)
|
||||
@@ -10,7 +19,14 @@ target_sources(Engine
|
||||
target_sources(Engine
|
||||
PUBLIC FILE_SET HEADERS
|
||||
FILES
|
||||
AABB.h
|
||||
BoxCollider.h
|
||||
Camera.h
|
||||
Component.h
|
||||
Collider.h
|
||||
MeshCollider.h
|
||||
RigidBody.h
|
||||
ShapeBase.h
|
||||
SphereCollider.h
|
||||
StaticMesh.h
|
||||
Transform.h)
|
||||
@@ -40,15 +40,15 @@ struct Camera
|
||||
void buildProjectionMatrix();
|
||||
Math::Matrix4 viewMatrix;
|
||||
Math::Matrix4 projectionMatrix;
|
||||
//Transforms relative to actor
|
||||
float yaw;
|
||||
float pitch;
|
||||
private:
|
||||
bool bNeedsViewBuild;
|
||||
bool bNeedsProjectionBuild;
|
||||
|
||||
Gfx::PViewport viewport;
|
||||
|
||||
//Transforms relative to actor
|
||||
float yaw;
|
||||
float pitch;
|
||||
};
|
||||
} // namespace Component
|
||||
} // namespace Seele
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
#include "Collider.h"
|
||||
|
||||
using namespace Seele;
|
||||
using namespace Seele::Component;
|
||||
|
||||
Collider Collider::transform(const Transform& transform) const
|
||||
{
|
||||
return Collider{
|
||||
.type = this->type,
|
||||
.boundingbox = this->boundingbox.getTransformedBox(transform.toMatrix()),
|
||||
.physicsMesh = this->physicsMesh.transform(transform),
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
#include "AABB.h"
|
||||
#include "ShapeBase.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
namespace Component
|
||||
{
|
||||
enum class ColliderType
|
||||
{
|
||||
STATIC,
|
||||
DYNAMIC,
|
||||
};
|
||||
struct Collider
|
||||
{
|
||||
ColliderType type;
|
||||
AABB boundingbox;
|
||||
ShapeBase physicsMesh;
|
||||
Collider transform(const Transform& transform) const;
|
||||
};
|
||||
} // namespace Component
|
||||
} // namespace Seele
|
||||
@@ -0,0 +1,17 @@
|
||||
#pragma once
|
||||
#include "AABB.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
namespace Component
|
||||
{
|
||||
struct RigidBody
|
||||
{
|
||||
float mass = 1.0f;
|
||||
Math::Vector force;
|
||||
Math::Vector torque;
|
||||
Math::Vector linearMomentum;
|
||||
Math::Vector angularMomentum;
|
||||
};
|
||||
} // namespace Component
|
||||
} // namespace Seele
|
||||
@@ -0,0 +1,226 @@
|
||||
#include "ShapeBase.h"
|
||||
|
||||
using namespace Seele;
|
||||
using namespace Seele::Component;
|
||||
|
||||
|
||||
//https://people.eecs.berkeley.edu/~jfc/mirtich/massProps.html
|
||||
struct ComputationState
|
||||
{
|
||||
// compute physics properties
|
||||
int A; /* alpha */
|
||||
int B; /* beta */
|
||||
int C; /* gamma */
|
||||
|
||||
/* projection integrals */
|
||||
float P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
|
||||
|
||||
/* face integrals */
|
||||
float Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
|
||||
|
||||
/* volume integrals */
|
||||
float T0;
|
||||
Math::Vector T1, T2, TP;
|
||||
};
|
||||
|
||||
struct Face
|
||||
{
|
||||
StaticArray<Math::Vector, 3> vertices;
|
||||
Math::Vector normal;
|
||||
float w;
|
||||
};
|
||||
|
||||
void computeProjectionIntegrals(Face& f, ComputationState& state)
|
||||
{
|
||||
state.P1 = state.Pa = state.Pb = state.Paa = state.Pab = state.Pbb = state.Paaa = state.Paab = state.Pabb = state.Pbbb = 0.0;
|
||||
for(uint32_t i = 0; i < 3; ++i)
|
||||
{
|
||||
float a0 = f.vertices[i][state.A];
|
||||
float b0 = f.vertices[i][state.B];
|
||||
float a1 = f.vertices[(i+1)%3][state.A];
|
||||
float b1 = f.vertices[(i+1)%3][state.B];
|
||||
|
||||
float da = a1 - a0;
|
||||
float db = b1 - b0;
|
||||
|
||||
float a0_2 = a0 * a0, a0_3 = a0_2 * a0, a0_4 = a0_3 * a0;
|
||||
float b0_2 = b0 * b0, b0_3 = b0_2 * b0, b0_4 = b0_3 * b0;
|
||||
float a1_2 = a1 * a1, a1_3 = a1_2 * a1;
|
||||
float b1_2 = b1 * b1, b1_3 = b1_2 * b1;
|
||||
|
||||
float C1 = a1 + a0;
|
||||
float Ca = a1 * C1 + a0_2;
|
||||
float Caa = a1 * Ca + a0_3;
|
||||
float Caaa = a1 * Caa + a0_4;
|
||||
float Cb = b1 * (b1 + b0) + b0_2;
|
||||
float Cbb = b1 * Cb + b0_3;
|
||||
float Cbbb = b1 * Cbb + b0_4;
|
||||
float Cab = 3 * a1_2 + 2 * a1 * a0 + a0_2;
|
||||
float Kab = a1_2 + 2 * a1 * a0 + 3 * a0_2;
|
||||
float Caab = a0 * Cab + 4 * a1_3;
|
||||
float Kaab = a1 * Kab + 4 * a0_3;
|
||||
float Cabb = 4 * b1_3 + 3 * b1_2 * b0 + 2 * b1 * b0_2 + b0_3;
|
||||
float Kabb = b1_3 + 2 * b1_2 * b0 + 3 * b1 * b0_2 + 4 * b0_3;
|
||||
|
||||
state.P1 += db * C1;
|
||||
state.Pa += db * Ca;
|
||||
state.Paa += db * Caa;
|
||||
state.Paaa += db * Caaa;
|
||||
state.Pb += da * Cb;
|
||||
state.Pbb += da * Cbb;
|
||||
state.Pbbb += da * Cbbb;
|
||||
state.Pab += db * (b1 * Cab + b0 * Kab);
|
||||
state.Paab += db * (b1 * Caab + b0 * Kaab);
|
||||
state.Pabb += da * (a1 * Cabb + a0 * Kabb);
|
||||
}
|
||||
|
||||
state.P1 /= 2.0;
|
||||
state.Pa /= 6.0;
|
||||
state.Paa /= 12.0;
|
||||
state.Paaa /= 20.0;
|
||||
state.Pb /= -6.0;
|
||||
state.Pbb /= -12.0;
|
||||
state.Pbbb /= -20.0;
|
||||
state.Pab /= 24.0;
|
||||
state.Paab /= 60.0;
|
||||
state.Pabb /= -60.0;
|
||||
}
|
||||
|
||||
void computeFaceIntegrals(Face& f, ComputationState& state)
|
||||
{
|
||||
computeProjectionIntegrals(f, state);
|
||||
float k1 = 1.0f/f.normal[state.C];
|
||||
float k2 = k1 * k1;
|
||||
float k3 = k2 * k1;
|
||||
float k4 = k3 * k1;
|
||||
|
||||
glm::vec3 n = f.normal;
|
||||
float w = f.w;
|
||||
|
||||
state.Fa = k1 * state.Pa;
|
||||
state.Fb = k1 * state.Pb;
|
||||
state.Fc = -k2 * (n[state.A] * state.Pa + n[state.B] * state.Pb + f.w * state.P1);
|
||||
|
||||
state.Faa = k1 * state.Paa;
|
||||
state.Fbb = k1 * state.Pbb;
|
||||
state.Fcc = k3 * (n[state.A] * n[state.A] * state.Paa
|
||||
+ 2 * n[state.A] * n[state.B] * state.Pab
|
||||
+ n[state.B] * n[state.B] * state.Pbb
|
||||
+ w * (2 * (n[state.A] * state.Pa + n[state.B] * state.Pb) + w * state.P1));
|
||||
|
||||
state.Faaa = k1 * state.Paaa;
|
||||
state.Fbbb = k1 * state.Pbbb;
|
||||
state.Fccc = -k4 * (n[state.A] * n[state.A] * n[state.A] * state.Paaa
|
||||
+ 3 * n[state.A] * n[state.A] * n[state.B] * state.Paab
|
||||
+ 3 * n[state.A] * n[state.B] * n[state.B] * state.Pabb
|
||||
+ 3 * n[state.B] * n[state.B] * n[state.B] * state.Pbbb
|
||||
+ 3 * w * (n[state.A] * n[state.A] * state.Paa
|
||||
+ 2 * n[state.A] * n[state.B] * state.Pa
|
||||
+ n[state.B] * n[state.B] * state.Pbb)
|
||||
+ w * w *(3 * (n[state.A]*state.Pa + n[state.B]*state.Pb) + w * state.P1)
|
||||
);
|
||||
|
||||
state.Faab = k1 * state.Paab;
|
||||
state.Fbbc = -k2 * (n[state.A] * state.Paab + n[state.B] * state.Pbbb + w * state.Pbb);
|
||||
state.Fcca = k3 * (n[state.A] * n[state.A] * state.Paa + 2 * n[state.A] * n[state.B] * state.Paab + n[state.B] * n[state.B] * state.Pabb
|
||||
+ w * (2 * (n[state.A] * state.Paa + n[state.B] * state.Pab) + w * state.Pa));
|
||||
}
|
||||
|
||||
void computeVolumeIntegrals(const Array<Math::Vector> vertices, const Array<uint32>& indices, ComputationState& state)
|
||||
{
|
||||
std::memset(&state, 0, sizeof(ComputationState));
|
||||
for (size_t i = 0; i < indices.size(); i+=3)
|
||||
{
|
||||
Face f;
|
||||
f.vertices = {
|
||||
vertices[indices[i]],
|
||||
vertices[indices[i+1]],
|
||||
vertices[indices[i+2]],
|
||||
};
|
||||
|
||||
Math::Vector e1 = f.vertices[2] - f.vertices[0];
|
||||
Math::Vector e2 = f.vertices[1] - f.vertices[0];
|
||||
f.normal = glm::normalize(glm::cross(e1, e2));
|
||||
f.w = - f.normal.x * f.vertices[0].x
|
||||
- f.normal.y * f.vertices[0].y
|
||||
- f.normal.z * f.vertices[0].z;
|
||||
|
||||
float nx = std::abs(f.normal.x);
|
||||
float ny = std::abs(f.normal.y);
|
||||
float nz = std::abs(f.normal.z);
|
||||
if (nx > ny && nx > nz) state.C = 0;
|
||||
else state.C = (ny > nz) ? 1 : 2;
|
||||
state.A = (state.C + 1) % 3;
|
||||
state.B = (state.A + 1) % 3;
|
||||
|
||||
computeFaceIntegrals(f, state);
|
||||
|
||||
state.T0 += f.normal.x * ((state.A == 0) ? state.Fa : ((state.B == 0) ? state.Fb : state.Fc));
|
||||
state.T1[state.A] += f.normal[state.A] * state.Faa;
|
||||
state.T1[state.B] += f.normal[state.B] * state.Fbb;
|
||||
state.T1[state.C] += f.normal[state.C] * state.Fcc;
|
||||
state.T2[state.A] += f.normal[state.A] * state.Faaa;
|
||||
state.T2[state.B] += f.normal[state.B] * state.Fbbb;
|
||||
state.T2[state.C] += f.normal[state.C] * state.Fccc;
|
||||
state.TP[state.A] += f.normal[state.A] * state.Faab;
|
||||
state.TP[state.B] += f.normal[state.B] * state.Fbbc;
|
||||
state.TP[state.C] += f.normal[state.C] * state.Fcca;
|
||||
}
|
||||
state.T1 /= 2.0f;
|
||||
state.T2 /= 3.0f;
|
||||
state.TP /= 2.0f;
|
||||
}
|
||||
|
||||
void computePhysicsParamsForMesh(Array<Math::Vector>& vertices, const Array<uint32_t>& indices, Math::Matrix3& bodyInertia, Math::Vector& centerOfMass, float& mass)
|
||||
{
|
||||
ComputationState state;
|
||||
computeVolumeIntegrals(vertices, indices, state);
|
||||
float density = 1;
|
||||
mass = density * state.T0;
|
||||
Math::Vector r = state.T1 / state.T0;
|
||||
centerOfMass = r;
|
||||
bodyInertia[0][0] = density * (state.T2.y + state.T2.z);
|
||||
bodyInertia[1][1] = density * (state.T2.z + state.T2.x);
|
||||
bodyInertia[2][2] = density * (state.T2.x + state.T2.y);
|
||||
bodyInertia[0][1] = bodyInertia[1][0] = - density * state.TP.x;
|
||||
bodyInertia[1][2] = bodyInertia[2][1] = - density * state.TP.y;
|
||||
bodyInertia[2][1] = bodyInertia[1][2] = - density * state.TP.z;
|
||||
|
||||
bodyInertia[0][0] -= mass * (r.y * r.y + r.z * r.z);
|
||||
bodyInertia[1][1] -= mass * (r.z * r.z + r.x * r.x);
|
||||
bodyInertia[2][2] -= mass * (r.x * r.x + r.y * r.y);
|
||||
bodyInertia[0][1] = bodyInertia[1][0] += mass * r.x * r.y;
|
||||
bodyInertia[1][2] = bodyInertia[2][1] += mass * r.y * r.z;
|
||||
bodyInertia[2][1] = bodyInertia[1][2] += mass * r.z * r.x;
|
||||
}
|
||||
|
||||
ShapeBase::ShapeBase(Array<Math::Vector> vertices, Array<uint32> indices)
|
||||
: vertices(vertices)
|
||||
, indices(indices)
|
||||
{
|
||||
computePhysicsParamsForMesh(vertices, indices, bodyInertia, centerOfMass, mass);
|
||||
}
|
||||
|
||||
ShapeBase ShapeBase::transform(const Component::Transform& transform) const
|
||||
{
|
||||
ShapeBase result = *this;
|
||||
for(auto& vert : result.vertices)
|
||||
{
|
||||
vert = transform.toMatrix() * Math::Vector4(vert, 1.0f);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void ShapeBase::addCollider(Array<Math::Vector> verts, Array<uint32> inds, Math::Matrix4 matrix)
|
||||
{
|
||||
size_t indOffset = vertices.size();
|
||||
for(auto vert : verts)
|
||||
{
|
||||
vertices.add(Math::Vector(matrix * Math::Vector4(vert, 1.0f)));
|
||||
}
|
||||
for(auto ind : inds)
|
||||
{
|
||||
indices.add(ind + static_cast<uint32>(indOffset));
|
||||
}
|
||||
computePhysicsParamsForMesh(vertices, indices, bodyInertia, centerOfMass, mass);
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#include "Containers/Array.h"
|
||||
#include "Transform.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
namespace Component
|
||||
{
|
||||
struct ShapeBase
|
||||
{
|
||||
ShapeBase(Array<Math::Vector> vertices, Array<uint32> indices);
|
||||
ShapeBase transform(const Component::Transform& transform) const;
|
||||
void addCollider(Array<Math::Vector> vertices, Array<uint32> indices, Math::Matrix4 matrix);
|
||||
Math::Vector centerOfMass;
|
||||
float mass;
|
||||
Math::Matrix3 bodyInertia;
|
||||
Array<Math::Vector> vertices;
|
||||
Array<uint32> indices;
|
||||
};
|
||||
} // namespace Component
|
||||
} // namespace Seele
|
||||
@@ -4,31 +4,57 @@ using namespace Seele::Component;
|
||||
|
||||
DEFINE_COMPONENT(Transform)
|
||||
|
||||
|
||||
void Transform::setPosition(Math::Vector pos)
|
||||
{
|
||||
transform.setPosition(pos);
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
void Transform::setRotation(Math::Quaternion quat)
|
||||
{
|
||||
transform.setRotation(quat);
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
void Transform::setScale(Math::Vector scale)
|
||||
{
|
||||
transform.setScale(scale);
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
void Transform::setRelativeLocation(Math::Vector location)
|
||||
{
|
||||
transform = Math::Transform(location, transform.getRotation(), transform.getScale());
|
||||
dirty = true;
|
||||
}
|
||||
void Transform::setRelativeRotation(Math::Vector rotation)
|
||||
{
|
||||
transform = Math::Transform(transform.getPosition(), Math::Quaternion(rotation), transform.getScale());
|
||||
dirty = true;
|
||||
}
|
||||
void Transform::setRelativeRotation(Math::Quaternion rotation)
|
||||
{
|
||||
transform = Math::Transform(transform.getPosition(), rotation, transform.getScale());
|
||||
dirty = true;
|
||||
}
|
||||
void Transform::setRelativeScale(Math::Vector scale)
|
||||
{
|
||||
transform = Math::Transform(transform.getPosition(), transform.getRotation(), scale);
|
||||
dirty = true;
|
||||
}
|
||||
void Transform::addRelativeLocation(Math::Vector translation)
|
||||
{
|
||||
transform = Math::Transform(transform.getPosition() + translation, transform.getRotation(), transform.getScale());
|
||||
dirty = true;
|
||||
}
|
||||
void Transform::addRelativeRotation(Math::Vector rotation)
|
||||
{
|
||||
transform = Math::Transform(transform.getPosition(), transform.getRotation() * Math::Quaternion(rotation), transform.getScale());
|
||||
dirty = true;
|
||||
}
|
||||
void Transform::addRelativeRotation(Math::Quaternion rotation)
|
||||
{
|
||||
transform = Math::Transform(transform.getPosition(), transform.getRotation() * rotation, transform.getScale());
|
||||
dirty = true;
|
||||
}
|
||||
@@ -8,23 +8,6 @@ namespace Component
|
||||
{
|
||||
struct Transform
|
||||
{
|
||||
Transform() {}
|
||||
Transform(const Transform& other) : transform(other.transform) {}
|
||||
Transform(Transform&& other) : transform(other.transform) {}
|
||||
Transform& operator=(const Transform& other) {
|
||||
if (&other != this)
|
||||
{
|
||||
transform = other.transform;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
Transform& operator=(Transform&& other) {
|
||||
if(&other != this)
|
||||
{
|
||||
transform = std::move(other.transform);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
Math::Vector getPosition() const { return transform.getPosition(); }
|
||||
Math::Quaternion getRotation() const { return transform.getRotation(); }
|
||||
Math::Vector getScale() const { return transform.getScale(); }
|
||||
@@ -35,6 +18,13 @@ struct Transform
|
||||
|
||||
Math::Matrix4 toMatrix() const { return transform.toMatrix(); }
|
||||
|
||||
bool isDirty() const { return dirty; }
|
||||
void clean() { dirty = false; }
|
||||
|
||||
void setPosition(Math::Vector pos);
|
||||
void setRotation(Math::Quaternion quat);
|
||||
void setScale(Math::Vector scale);
|
||||
|
||||
void setRelativeLocation(Math::Vector location);
|
||||
void setRelativeRotation(Math::Quaternion rotation);
|
||||
void setRelativeRotation(Math::Vector rotation);
|
||||
@@ -44,6 +34,7 @@ struct Transform
|
||||
void addRelativeRotation(Math::Quaternion rotation);
|
||||
void addRelativeRotation(Math::Vector rotation);
|
||||
private:
|
||||
bool dirty = true;
|
||||
Math::Transform transform;
|
||||
};
|
||||
DECLARE_COMPONENT(Transform)
|
||||
|
||||
@@ -26,53 +26,66 @@ public:
|
||||
using reference = X&;
|
||||
using pointer = X*;
|
||||
|
||||
IteratorBase(X *x = nullptr)
|
||||
constexpr IteratorBase(X *x = nullptr)
|
||||
: p(x)
|
||||
{
|
||||
}
|
||||
reference operator*() const
|
||||
constexpr reference operator*() const
|
||||
{
|
||||
return *p;
|
||||
}
|
||||
pointer operator->() const
|
||||
constexpr pointer operator->() const
|
||||
{
|
||||
return p;
|
||||
}
|
||||
inline bool operator!=(const IteratorBase &other) const
|
||||
constexpr bool operator!=(const IteratorBase &other) const
|
||||
{
|
||||
return p != other.p;
|
||||
}
|
||||
inline bool operator==(const IteratorBase &other) const
|
||||
constexpr bool operator==(const IteratorBase &other) const
|
||||
{
|
||||
return p == other.p;
|
||||
}
|
||||
inline int operator-(const IteratorBase &other) const
|
||||
constexpr IteratorBase operator+(size_t other) const
|
||||
{
|
||||
IteratorBase tmp(*this);
|
||||
tmp.p += other;
|
||||
return tmp;
|
||||
}
|
||||
constexpr int operator-(const IteratorBase &other) const
|
||||
{
|
||||
return (int)(p - other.p);
|
||||
}
|
||||
IteratorBase &operator++()
|
||||
constexpr bool operator<(const IteratorBase& other) const
|
||||
{
|
||||
return p < other.p;
|
||||
}
|
||||
constexpr IteratorBase operator-(difference_type diff) const
|
||||
{
|
||||
return IteratorBase(p - diff);
|
||||
}
|
||||
constexpr IteratorBase &operator++()
|
||||
{
|
||||
p++;
|
||||
return *this;
|
||||
}
|
||||
IteratorBase &operator--()
|
||||
constexpr IteratorBase &operator--()
|
||||
{
|
||||
p--;
|
||||
return *this;
|
||||
}
|
||||
IteratorBase operator++(int)
|
||||
constexpr IteratorBase operator++(int)
|
||||
{
|
||||
IteratorBase tmp(*this);
|
||||
++*this;
|
||||
return tmp;
|
||||
}
|
||||
IteratorBase operator--(int)
|
||||
constexpr IteratorBase operator--(int)
|
||||
{
|
||||
IteratorBase tmp(*this);
|
||||
--*this;
|
||||
return tmp;
|
||||
}
|
||||
|
||||
private:
|
||||
X *p;
|
||||
};
|
||||
@@ -100,7 +113,6 @@ public:
|
||||
{
|
||||
_data = allocateArray(DEFAULT_ALLOC_SIZE);
|
||||
assert(_data != nullptr);
|
||||
markIteratorDirty();
|
||||
}
|
||||
|
||||
constexpr explicit Array(const allocator_type& alloc) noexcept
|
||||
@@ -110,7 +122,6 @@ public:
|
||||
{
|
||||
_data = allocateArray(DEFAULT_ALLOC_SIZE);
|
||||
assert(_data != nullptr);
|
||||
markIteratorDirty();
|
||||
}
|
||||
constexpr Array(size_type size, const value_type& value, const allocator_type& alloc = allocator_type())
|
||||
: arraySize(size)
|
||||
@@ -123,7 +134,6 @@ public:
|
||||
{
|
||||
_data[i] = value;
|
||||
}
|
||||
markIteratorDirty();
|
||||
}
|
||||
constexpr explicit Array(size_type size, const allocator_type& alloc = allocator_type())
|
||||
: arraySize(size)
|
||||
@@ -136,7 +146,6 @@ public:
|
||||
{
|
||||
std::allocator_traits<allocator_type>::construct(allocator, &_data[i]);
|
||||
}
|
||||
markIteratorDirty();
|
||||
}
|
||||
constexpr Array(std::initializer_list<T> init, const allocator_type& alloc = allocator_type())
|
||||
: arraySize(init.size())
|
||||
@@ -145,7 +154,6 @@ public:
|
||||
{
|
||||
_data = allocateArray(init.size());
|
||||
assert(_data != nullptr);
|
||||
markIteratorDirty();
|
||||
std::uninitialized_copy(init.begin(), init.end(), begin());
|
||||
}
|
||||
Array(const Array &other)
|
||||
@@ -155,7 +163,6 @@ public:
|
||||
{
|
||||
_data = allocateArray(other.allocated);
|
||||
assert(_data != nullptr);
|
||||
markIteratorDirty();
|
||||
std::uninitialized_copy(other.begin(), other.end(), begin());
|
||||
}
|
||||
Array(Array &&other) noexcept
|
||||
@@ -167,7 +174,6 @@ public:
|
||||
other._data = nullptr;
|
||||
other.allocated = 0;
|
||||
other.arraySize = 0;
|
||||
markIteratorDirty();
|
||||
}
|
||||
Array &operator=(const Array &other)
|
||||
{
|
||||
@@ -192,7 +198,6 @@ public:
|
||||
allocated = other.allocated;
|
||||
}
|
||||
arraySize = other.arraySize;
|
||||
markIteratorDirty();
|
||||
std::uninitialized_copy(other.begin(), other.end(), begin());
|
||||
}
|
||||
return *this;
|
||||
@@ -214,7 +219,6 @@ public:
|
||||
arraySize = std::move(other.arraySize);
|
||||
_data = other._data;
|
||||
other._data = nullptr;
|
||||
markIteratorDirty();
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
@@ -222,6 +226,7 @@ public:
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
constexpr iterator find(const value_type &item) noexcept
|
||||
{
|
||||
@@ -232,7 +237,7 @@ public:
|
||||
return iterator(&_data[i]);
|
||||
}
|
||||
}
|
||||
return endIt;
|
||||
return end();
|
||||
}
|
||||
[[nodiscard]]
|
||||
constexpr iterator find(value_type&& item) noexcept
|
||||
@@ -244,40 +249,72 @@ public:
|
||||
return iterator(&_data[i]);
|
||||
}
|
||||
}
|
||||
return endIt;
|
||||
return end();
|
||||
}
|
||||
[[nodiscard]]
|
||||
constexpr const_iterator find(const value_type &item) const noexcept
|
||||
{
|
||||
for (size_type i = 0; i < arraySize; ++i)
|
||||
{
|
||||
if (_data[i] == item)
|
||||
{
|
||||
return const_iterator(&_data[i]);
|
||||
}
|
||||
}
|
||||
return end();
|
||||
}
|
||||
[[nodiscard]]
|
||||
constexpr const_iterator find(value_type&& item) const noexcept
|
||||
{
|
||||
for (size_type i = 0; i < arraySize; ++i)
|
||||
{
|
||||
if (_data[i] == item)
|
||||
{
|
||||
return const_iterator(&_data[i]);
|
||||
}
|
||||
}
|
||||
return end();
|
||||
}
|
||||
template<class Pred>
|
||||
requires std::predicate<Pred, value_type>
|
||||
constexpr iterator find(Pred pred) const noexcept
|
||||
constexpr const_iterator find(Pred pred) const noexcept
|
||||
{
|
||||
for (size_type i = 0; i < arraySize; ++i)
|
||||
{
|
||||
if(pred(_data[i]))
|
||||
{
|
||||
return iterator(&_data[i]);
|
||||
return const_iterator(&_data[i]);
|
||||
}
|
||||
}
|
||||
return endIt;
|
||||
return end();
|
||||
}
|
||||
constexpr allocator_type get_allocator() const noexcept
|
||||
{
|
||||
return allocator;
|
||||
}
|
||||
constexpr iterator begin() const noexcept
|
||||
constexpr iterator begin() noexcept
|
||||
{
|
||||
return beginIt;
|
||||
return iterator(_data);
|
||||
}
|
||||
constexpr iterator end() const noexcept
|
||||
constexpr iterator end() noexcept
|
||||
{
|
||||
return endIt;
|
||||
return iterator(_data + arraySize);
|
||||
}
|
||||
constexpr const_iterator begin() const noexcept
|
||||
{
|
||||
return const_iterator(_data);
|
||||
}
|
||||
constexpr const_iterator end() const noexcept
|
||||
{
|
||||
return const_iterator(_data + arraySize);
|
||||
}
|
||||
constexpr const_iterator cbegin() const noexcept
|
||||
{
|
||||
return beginIt;
|
||||
return const_iterator(_data);
|
||||
}
|
||||
constexpr const_iterator cend() const noexcept
|
||||
{
|
||||
return endIt;
|
||||
return const_iterator(_data + arraySize);
|
||||
}
|
||||
constexpr reference add(const value_type &item = value_type())
|
||||
{
|
||||
@@ -297,7 +334,7 @@ public:
|
||||
constexpr reference addUnique(const value_type &item = value_type())
|
||||
{
|
||||
iterator it;
|
||||
if((it = std::move(find(item))) != endIt)
|
||||
if((it = std::move(find(item))) != end())
|
||||
{
|
||||
return *it;
|
||||
}
|
||||
@@ -318,7 +355,6 @@ public:
|
||||
_data = tempArray;
|
||||
}
|
||||
std::allocator_traits<allocator_type>::construct(allocator, &_data[arraySize++], arguments...);
|
||||
markIteratorDirty();
|
||||
return _data[arraySize - 1];
|
||||
}
|
||||
template<std::predicate Pred>
|
||||
@@ -336,7 +372,11 @@ public:
|
||||
}
|
||||
constexpr void remove(iterator it, bool keepOrder = true)
|
||||
{
|
||||
removeAt(it - beginIt, keepOrder);
|
||||
removeAt(it - begin(), keepOrder);
|
||||
}
|
||||
constexpr void remove(const_iterator it, bool keepOrder = true)
|
||||
{
|
||||
removeAt(it - cbegin(), keepOrder);
|
||||
}
|
||||
constexpr void removeAt(size_type index, bool keepOrder = true)
|
||||
{
|
||||
@@ -352,7 +392,6 @@ public:
|
||||
_data[index] = std::move(_data[arraySize - 1]);
|
||||
}
|
||||
std::allocator_traits<allocator_type>::destroy(allocator, &_data[--arraySize]);
|
||||
markIteratorDirty();
|
||||
}
|
||||
constexpr void resize(size_type newSize)
|
||||
{
|
||||
@@ -376,15 +415,14 @@ public:
|
||||
_data = nullptr;
|
||||
arraySize = 0;
|
||||
allocated = 0;
|
||||
markIteratorDirty();
|
||||
}
|
||||
constexpr size_type indexOf(iterator iterator)
|
||||
{
|
||||
return iterator - beginIt;
|
||||
return iterator - begin();
|
||||
}
|
||||
constexpr size_type indexOf(const_iterator iterator) const
|
||||
{
|
||||
return iterator.p - beginIt.p;
|
||||
return iterator.p - begin().p;
|
||||
}
|
||||
constexpr size_type indexOf(T& t)
|
||||
{
|
||||
@@ -408,9 +446,8 @@ public:
|
||||
if(new_cap > allocated)
|
||||
{
|
||||
T* temp = allocateArray(new_cap);
|
||||
std::uninitialized_move_n(beginIt, arraySize, temp);
|
||||
std::uninitialized_move_n(begin(), arraySize, temp);
|
||||
_data = temp;
|
||||
markIteratorDirty();
|
||||
}
|
||||
allocated = new_cap;
|
||||
}
|
||||
@@ -435,7 +472,6 @@ public:
|
||||
constexpr void pop()
|
||||
{
|
||||
std::allocator_traits<allocator_type>::destroy(allocator, &_data[--arraySize]);
|
||||
markIteratorDirty();
|
||||
}
|
||||
constexpr reference operator[](size_type index)
|
||||
{
|
||||
@@ -466,11 +502,6 @@ private:
|
||||
|
||||
return geometric; // geometric growth is sufficient
|
||||
}
|
||||
void markIteratorDirty()
|
||||
{
|
||||
beginIt = Iterator(_data);
|
||||
endIt = Iterator(_data + arraySize);
|
||||
}
|
||||
[[nodiscard]]
|
||||
T* allocateArray(size_type size)
|
||||
{
|
||||
@@ -498,7 +529,6 @@ private:
|
||||
_data = tempArray;
|
||||
}
|
||||
std::allocator_traits<allocator_type>::construct(allocator, &_data[arraySize++], std::forward<Type>(t));
|
||||
markIteratorDirty();
|
||||
return _data[arraySize - 1];
|
||||
}
|
||||
template<typename Type>
|
||||
@@ -545,12 +575,9 @@ private:
|
||||
allocated = newSize;
|
||||
_data = newData;
|
||||
}
|
||||
markIteratorDirty();
|
||||
}
|
||||
size_type arraySize = 0;
|
||||
size_type allocated = 0;
|
||||
Iterator beginIt;
|
||||
Iterator endIt;
|
||||
T *_data = nullptr;
|
||||
allocator_type allocator;
|
||||
};
|
||||
|
||||
@@ -1,13 +1,15 @@
|
||||
target_sources(Engine
|
||||
PRIVATE
|
||||
Array.h
|
||||
List.h
|
||||
Map.h
|
||||
List.h)
|
||||
Pair.h)
|
||||
|
||||
|
||||
target_sources(Engine
|
||||
PUBLIC FILE_SET HEADERS
|
||||
FILES
|
||||
Array.h
|
||||
List.h
|
||||
Map.h
|
||||
List.h)
|
||||
Pair.h)
|
||||
@@ -1,32 +1,9 @@
|
||||
#pragma once
|
||||
#include <utility>
|
||||
#include "Array.h"
|
||||
|
||||
#include "Pair.h"
|
||||
namespace Seele
|
||||
{
|
||||
template <typename K, typename V>
|
||||
struct Pair
|
||||
{
|
||||
public:
|
||||
Pair()
|
||||
: key(K()), value(V())
|
||||
{}
|
||||
Pair(const Pair& other) = default;
|
||||
Pair(Pair&& other) = default;
|
||||
~Pair(){}
|
||||
Pair& operator=(const Pair& other) = default;
|
||||
Pair& operator=(Pair&& other) = default;
|
||||
template<class KeyType>
|
||||
explicit Pair(KeyType&& key)
|
||||
: key(std::forward<KeyType>(key)), value(V())
|
||||
{}
|
||||
template<class KeyType, class ValueType>
|
||||
explicit Pair(KeyType&& key, ValueType&& value)
|
||||
: key(std::forward<KeyType>(key)), value(std::forward<ValueType>(value))
|
||||
{}
|
||||
K key;
|
||||
V value;
|
||||
};
|
||||
template <typename K,
|
||||
typename V,
|
||||
typename Compare = std::less<K>,
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
template <typename K, typename V>
|
||||
struct Pair
|
||||
{
|
||||
public:
|
||||
Pair()
|
||||
: key(K()), value(V())
|
||||
{}
|
||||
Pair(const Pair& other) = default;
|
||||
Pair(Pair&& other) = default;
|
||||
~Pair(){}
|
||||
template<class KeyType>
|
||||
explicit Pair(KeyType&& key)
|
||||
: key(std::forward<KeyType>(key)), value(V())
|
||||
{}
|
||||
template<class KeyType, class ValueType>
|
||||
explicit Pair(KeyType&& key, ValueType&& value)
|
||||
: key(std::forward<KeyType>(key)), value(std::forward<ValueType>(value))
|
||||
{}
|
||||
Pair& operator=(const Pair& other) = default;
|
||||
Pair& operator=(Pair&& other) = default;
|
||||
constexpr friend bool operator<(const Pair& left, const Pair& right)
|
||||
{
|
||||
return left.key < right.key || (!(right.key < left.key) && left.value < right.value);
|
||||
}
|
||||
K key;
|
||||
V value;
|
||||
};
|
||||
} // namespace Seele
|
||||
@@ -34,7 +34,6 @@ target_sources(Engine
|
||||
PUBLIC FILE_SET HEADERS
|
||||
FILES
|
||||
VulkanAllocator.h
|
||||
VulkanBuffer.cpp
|
||||
VulkanCommandBuffer.h
|
||||
VulkanFramebuffer.h
|
||||
VulkanGraphics.h
|
||||
|
||||
@@ -63,19 +63,19 @@ ShaderBuffer::ShaderBuffer(PGraphics graphics, uint64 size, VkBufferUsageFlags u
|
||||
|
||||
ShaderBuffer::~ShaderBuffer()
|
||||
{
|
||||
PCmdBuffer cmdBuffer = graphics->getQueueCommands(owner)->getCommands();
|
||||
VkDevice device = graphics->getDevice();
|
||||
auto deletionLambda = [cmdBuffer, device](VkBuffer) -> void
|
||||
{
|
||||
//co_await cmdBuffer->asyncWait();
|
||||
//vkDestroyBuffer(device, buffer, nullptr);
|
||||
//co_return;
|
||||
};
|
||||
for (uint32 i = 0; i < numBuffers; ++i)
|
||||
{
|
||||
deletionLambda(buffers[i].buffer);
|
||||
buffers[i].allocation = nullptr;
|
||||
}
|
||||
//PCmdBuffer cmdBuffer = graphics->getQueueCommands(owner)->getCommands();
|
||||
//VkDevice device = graphics->getDevice();
|
||||
//auto deletionLambda = [cmdBuffer, device](VkBuffer) -> void
|
||||
//{
|
||||
// //co_await cmdBuffer->asyncWait();
|
||||
// //vkDestroyBuffer(device, buffer, nullptr);
|
||||
// //co_return;
|
||||
//};
|
||||
//for (uint32 i = 0; i < numBuffers; ++i)
|
||||
//{
|
||||
// deletionLambda(buffers[i].buffer);
|
||||
// buffers[i].allocation = nullptr;
|
||||
//}
|
||||
graphics = nullptr;
|
||||
}
|
||||
|
||||
|
||||
@@ -46,7 +46,7 @@ void MaterialAsset::load()
|
||||
auto stream = getReadStream();
|
||||
json j;
|
||||
stream >> j;
|
||||
materialName = j["name"].get<std::string>();
|
||||
materialName = j["name"].get<std::string>() + "Material";
|
||||
layout = WindowManager::getGraphics()->createDescriptorLayout(materialName + "Layout");
|
||||
//Shader file needs to conform to the slang standard, which prohibits _
|
||||
materialName.erase(std::remove(materialName.begin(), materialName.end(), '_'), materialName.end());
|
||||
|
||||
@@ -145,14 +145,31 @@ Vector Transform::getScale() const
|
||||
return Vector(scale);
|
||||
}
|
||||
|
||||
void Transform::setPosition(Math::Vector pos)
|
||||
{
|
||||
position = Vector4(pos, 0);
|
||||
}
|
||||
|
||||
void Transform::setRotation(Math::Quaternion quat)
|
||||
{
|
||||
rotation = quat;
|
||||
}
|
||||
|
||||
void Transform::setScale(Math::Vector s)
|
||||
{
|
||||
scale = Vector4(s, 0);
|
||||
}
|
||||
|
||||
Vector Transform::getForward() const
|
||||
{
|
||||
return glm::normalize(Vector(0, 0, 1) * rotation);
|
||||
}
|
||||
|
||||
Vector Transform::getRight() const
|
||||
{
|
||||
return glm::normalize(Vector(1, 0, 0) * rotation);
|
||||
}
|
||||
|
||||
Vector Transform::getUp() const
|
||||
{
|
||||
return glm::normalize(Vector(0, 1, 0) * rotation);
|
||||
|
||||
@@ -25,6 +25,11 @@ public:
|
||||
Vector getPosition() const;
|
||||
Quaternion getRotation() const;
|
||||
Vector getScale() const;
|
||||
|
||||
void setPosition(Math::Vector pos);
|
||||
void setRotation(Math::Quaternion quat);
|
||||
void setScale(Math::Vector scale);
|
||||
|
||||
Vector getForward() const;
|
||||
Vector getRight() const;
|
||||
Vector getUp() const;
|
||||
|
||||
@@ -5,15 +5,15 @@
|
||||
#include <map>
|
||||
|
||||
#define DEFINE_REF(x) \
|
||||
typedef RefPtr<x> P##x; \
|
||||
typedef UniquePtr<x> UP##x; \
|
||||
typedef WeakPtr<x> W##x;
|
||||
typedef ::Seele::RefPtr<x> P##x; \
|
||||
typedef ::Seele::UniquePtr<x> UP##x; \
|
||||
typedef ::Seele::WeakPtr<x> W##x;
|
||||
|
||||
#define DECLARE_REF(x) \
|
||||
class x; \
|
||||
typedef RefPtr<x> P##x; \
|
||||
typedef UniquePtr<x> UP##x; \
|
||||
typedef WeakPtr<x> W##x;
|
||||
typedef ::Seele::RefPtr<x> P##x; \
|
||||
typedef ::Seele::UniquePtr<x> UP##x; \
|
||||
typedef ::Seele::WeakPtr<x> W##x;
|
||||
|
||||
|
||||
#define DECLARE_NAME_REF(nmsp, x) \
|
||||
|
||||
@@ -0,0 +1,300 @@
|
||||
#include "BVH.h"
|
||||
|
||||
using namespace Seele;
|
||||
|
||||
void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps)
|
||||
{
|
||||
overlaps.clear();
|
||||
for(const auto& node : dynamicNodes)
|
||||
{
|
||||
traverseStaticTree(node.box, node.owner, staticRoot, overlaps);
|
||||
traverseDynamicTree(node.box, node.owner, dynamicRoot, overlaps);
|
||||
}
|
||||
}
|
||||
|
||||
void BVH::updateDynamicCollider(entt::entity entity, AABB aabb)
|
||||
{
|
||||
for(auto& aabbcenter : dynamicCollider)
|
||||
{
|
||||
if(aabbcenter.id == entity)
|
||||
{
|
||||
if(!aabbcenter.bb.contains(aabb))
|
||||
{
|
||||
// moved out of extended bounds
|
||||
reinsertCollider(entity, aabb);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
// new collider
|
||||
addDynamicCollider(entity, aabb);
|
||||
}
|
||||
|
||||
void BVH::traverseStaticTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps)
|
||||
{
|
||||
const Node& node = staticNodes[nodeIndex];
|
||||
if(!aabb.intersects(node.box))
|
||||
{
|
||||
return;
|
||||
}
|
||||
if(node.isLeaf)
|
||||
{
|
||||
overlaps.add(Pair<entt::entity, entt::entity>(source, node.owner));
|
||||
return;
|
||||
}
|
||||
traverseStaticTree(aabb, source, node.left, overlaps);
|
||||
traverseStaticTree(aabb, source, node.right, overlaps);
|
||||
}
|
||||
|
||||
void BVH::traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps)
|
||||
{
|
||||
const Node& node = staticNodes[nodeIndex];
|
||||
if(!aabb.intersects(node.box))
|
||||
{
|
||||
return;
|
||||
}
|
||||
if(node.isLeaf && node.owner != source)
|
||||
{
|
||||
overlaps.add(Pair<entt::entity, entt::entity>(source, node.owner));
|
||||
return;
|
||||
}
|
||||
traverseStaticTree(aabb, source, node.left, overlaps);
|
||||
traverseStaticTree(aabb, source, node.right, overlaps);
|
||||
}
|
||||
|
||||
void BVH::reinsertCollider(entt::entity entity, AABB aabb)
|
||||
{
|
||||
int32 nodeIndex = -1;
|
||||
for (int32 i = 0; i < dynamicNodes.size(); i++)
|
||||
{
|
||||
if(dynamicNodes[i].isLeaf && dynamicNodes[i].owner == entity)
|
||||
{
|
||||
nodeIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(nodeIndex == -1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
int32 parentIndex = dynamicNodes[nodeIndex].parentIndex;
|
||||
const Node& parent = dynamicNodes[parentIndex];
|
||||
int32 siblingIndex;
|
||||
if(parent.left == nodeIndex)
|
||||
{
|
||||
siblingIndex = parent.right;
|
||||
}
|
||||
else
|
||||
{
|
||||
siblingIndex = parent.left;
|
||||
}
|
||||
// the node to remove and their sibling share a parent
|
||||
// now that the node is removed, the parent is also useless
|
||||
// so the grandparent should point32 to the sibling directly instead of the parent
|
||||
Node& grandParent = dynamicNodes[parent.parentIndex];
|
||||
if(grandParent.left == parentIndex)
|
||||
{
|
||||
grandParent.left = siblingIndex;
|
||||
}
|
||||
else
|
||||
{
|
||||
grandParent.right = siblingIndex;
|
||||
}
|
||||
freeNode(parentIndex);
|
||||
freeNode(nodeIndex);
|
||||
addDynamicCollider(entity, aabb);
|
||||
}
|
||||
|
||||
void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
|
||||
{
|
||||
auto center = (aabb.max + aabb.min) / 2.0f;
|
||||
// enlarge box slightly to buffer movement
|
||||
aabb = AABB {
|
||||
.min = center + (aabb.min - center) * 1.1f,
|
||||
.max = center + (aabb.max - center) * 1.1f,
|
||||
};
|
||||
dynamicCollider.add(AABBCenter {
|
||||
.bb = aabb,
|
||||
.center = center,
|
||||
.id = entity
|
||||
});
|
||||
int32 leafIndex = allocateNode();
|
||||
Node newNode = Node {
|
||||
.box = aabb,
|
||||
.isLeaf = true,
|
||||
.owner = entity,
|
||||
};
|
||||
dynamicNodes[leafIndex] = newNode;
|
||||
|
||||
if (dynamicRoot == -1)
|
||||
{
|
||||
dynamicRoot = leafIndex;
|
||||
return;
|
||||
}
|
||||
|
||||
int32 bestSibling;
|
||||
float bestCost = std::numeric_limits<float>::max();
|
||||
findSibling(newNode, dynamicRoot, bestCost, bestSibling);
|
||||
|
||||
int32 oldParent = dynamicNodes[bestSibling].parentIndex;
|
||||
int32 newParent = allocateNode();
|
||||
dynamicNodes[newParent] = Node {
|
||||
.box = aabb.combine(dynamicNodes[bestSibling].box),
|
||||
.parentIndex = oldParent,
|
||||
.isLeaf = false,
|
||||
};
|
||||
|
||||
if(oldParent != -1)
|
||||
{
|
||||
if(dynamicNodes[oldParent].left == bestSibling)
|
||||
{
|
||||
dynamicNodes[oldParent].left = newParent;
|
||||
}
|
||||
else
|
||||
{
|
||||
dynamicNodes[oldParent].right = newParent;
|
||||
}
|
||||
dynamicNodes[newParent].left = bestSibling;
|
||||
dynamicNodes[newParent].right = leafIndex;
|
||||
dynamicNodes[bestSibling].parentIndex = newParent;
|
||||
dynamicNodes[leafIndex].parentIndex = newParent;
|
||||
}
|
||||
else
|
||||
{
|
||||
dynamicNodes[newParent].left = bestSibling;
|
||||
dynamicNodes[newParent].right = leafIndex;
|
||||
dynamicNodes[bestSibling].parentIndex = newParent;
|
||||
dynamicNodes[leafIndex].parentIndex = newParent;
|
||||
dynamicRoot = newParent;
|
||||
}
|
||||
int32 index = dynamicNodes[leafIndex].parentIndex;
|
||||
while(index != -1)
|
||||
{
|
||||
int32 left = dynamicNodes[index].left;
|
||||
int32 right = dynamicNodes[index].right;
|
||||
|
||||
dynamicNodes[index].box = dynamicNodes[left].box.combine(dynamicNodes[right].box);
|
||||
index = dynamicNodes[index].parentIndex;
|
||||
}
|
||||
}
|
||||
|
||||
void BVH::addStaticCollider(entt::entity entity, AABB boundingBox)
|
||||
{
|
||||
staticCollider.add(AABBCenter {
|
||||
.bb = boundingBox,
|
||||
.center = (boundingBox.min + boundingBox.max) / 2.0f,
|
||||
.id = entity,
|
||||
});
|
||||
staticNodes.clear();
|
||||
}
|
||||
|
||||
void BVH::findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result)
|
||||
{
|
||||
if(nodeIndex == -1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
float lowerBound = lowerBoundCost(newNode, nodeIndex);
|
||||
if(lowerBound < bestCost)
|
||||
{
|
||||
float cost = siblingCost(newNode, nodeIndex);
|
||||
if(cost < bestCost)
|
||||
{
|
||||
bestCost = cost;
|
||||
result = nodeIndex;
|
||||
}
|
||||
findSibling(newNode, dynamicNodes[nodeIndex].left, bestCost, result);
|
||||
findSibling(newNode, dynamicNodes[nodeIndex].right, bestCost, result);
|
||||
}
|
||||
}
|
||||
|
||||
float BVH::siblingCost(Node newNode, int32 siblingIndex)
|
||||
{
|
||||
const Node& sibling = dynamicNodes[siblingIndex];
|
||||
AABB newBox = sibling.box.combine(newNode.box);
|
||||
float cost = newBox.surfaceArea();
|
||||
int32 parentIndex = sibling.parentIndex;
|
||||
while(parentIndex != -1)
|
||||
{
|
||||
AABB parentBox = dynamicNodes[parentIndex].box;
|
||||
cost += parentBox.combine(newBox).surfaceArea() - parentBox.surfaceArea();
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
float BVH::lowerBoundCost(Node newNode, int32 branchIndex)
|
||||
{
|
||||
float cost = newNode.box.surfaceArea();
|
||||
while(branchIndex != -1)
|
||||
{
|
||||
AABB box = dynamicNodes[branchIndex].box;
|
||||
cost += box.combine(newNode.box).surfaceArea() - box.surfaceArea();
|
||||
branchIndex = dynamicNodes[branchIndex].parentIndex;
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
int32 BVH::splitNode(Array<AABBCenter> aabbs)
|
||||
{
|
||||
if(aabbs.size() == 1)
|
||||
{
|
||||
int32 leafIndex = static_cast<int32>(staticNodes.size());
|
||||
Node& leaf = staticNodes.add();
|
||||
leaf.box = aabbs[0].bb;
|
||||
leaf.left = -1;
|
||||
leaf.right = -1;
|
||||
leaf.owner = aabbs[0].id;
|
||||
return leafIndex;
|
||||
}
|
||||
AABB rootBox;
|
||||
for(size_t i = 0; i < aabbs.size(); ++i)
|
||||
{
|
||||
rootBox.adjust(aabbs[i].bb.min);
|
||||
rootBox.adjust(aabbs[i].bb.max);
|
||||
}
|
||||
float xlen = rootBox.max.x - rootBox.min.x;
|
||||
float ylen = rootBox.max.y - rootBox.min.y;
|
||||
float zlen = rootBox.max.z - rootBox.min.z;
|
||||
int32 longestAxis;
|
||||
if(xlen >= ylen && xlen >= zlen)
|
||||
{
|
||||
longestAxis = 0;
|
||||
}
|
||||
if(ylen >= xlen && ylen >= zlen)
|
||||
{
|
||||
longestAxis = 1;
|
||||
}
|
||||
if(zlen >= xlen && zlen >= ylen)
|
||||
{
|
||||
longestAxis = 2;
|
||||
}
|
||||
std::sort(aabbs.begin(), aabbs.end(), [longestAxis](AABBCenter lhs, AABBCenter rhs){ return lhs.center[longestAxis] < rhs.center[longestAxis];});
|
||||
Array<AABBCenter> left((aabbs.size()+1)/2);
|
||||
Array<AABBCenter> right(aabbs.size()/2);
|
||||
std::copy(aabbs.begin(), aabbs.begin()+left.size(), left.begin());
|
||||
std::copy(aabbs.begin()+left.size(), aabbs.end(), right.begin());
|
||||
int32 rootIndex = static_cast<int32>(staticNodes.size());
|
||||
Node& rootNode = staticNodes.add();
|
||||
rootNode.box = rootBox;
|
||||
rootNode.left = splitNode(std::move(left));
|
||||
rootNode.right = splitNode(std::move(right));
|
||||
return rootIndex;
|
||||
}
|
||||
int32 BVH::allocateNode()
|
||||
{
|
||||
for (int32 i = 0; i < dynamicNodes.size(); i++)
|
||||
{
|
||||
if(!dynamicNodes[i].isValid)
|
||||
{
|
||||
return i;
|
||||
}
|
||||
}
|
||||
int32 newLeaf = static_cast<int32>(dynamicNodes.size());
|
||||
dynamicNodes.add();
|
||||
return newLeaf;
|
||||
}
|
||||
void BVH::freeNode(int32 nodeIndex)
|
||||
{
|
||||
dynamicNodes[nodeIndex].isValid = false;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
#pragma once
|
||||
#include <entt/entt.hpp>
|
||||
#include "Containers/Array.h"
|
||||
#include "Component/AABB.h"
|
||||
#include "Component/Collider.h"
|
||||
#include "Containers/Pair.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
class BVH
|
||||
{
|
||||
public:
|
||||
void findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps);
|
||||
void updateDynamicCollider(entt::entity entity, AABB aabb);
|
||||
private:
|
||||
struct AABBCenter
|
||||
{
|
||||
AABB bb;
|
||||
Math::Vector center;
|
||||
entt::entity id;
|
||||
};
|
||||
struct Node
|
||||
{
|
||||
AABB box;
|
||||
int32 parentIndex = -1;
|
||||
int32 left = -1;
|
||||
int32 right = -1;
|
||||
bool isLeaf;
|
||||
bool isValid = true;
|
||||
entt::entity owner;
|
||||
};
|
||||
void traverseStaticTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps);
|
||||
void traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps);
|
||||
void reinsertCollider(entt::entity, AABB aabb);
|
||||
void removeCollider(entt::entity entity);
|
||||
void addDynamicCollider(entt::entity entity, AABB aabb);
|
||||
void addStaticCollider(entt::entity entity, AABB aabb);
|
||||
void findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result);
|
||||
float siblingCost(Node newNode, int32 siblingIndex);
|
||||
float lowerBoundCost(Node newNode, int32 branchIndex);
|
||||
int32 splitNode(Array<AABBCenter> aabbs);
|
||||
int32 allocateNode();
|
||||
void freeNode(int32 nodeIndex);
|
||||
Array<Node> dynamicNodes;
|
||||
Array<Node> staticNodes;
|
||||
Array<AABBCenter> dynamicCollider;
|
||||
Array<AABBCenter> staticCollider;
|
||||
int32 staticRoot = -1;
|
||||
int32 dynamicRoot = -1;
|
||||
};
|
||||
} // namespace Seele
|
||||
@@ -0,0 +1,15 @@
|
||||
target_sources(Engine
|
||||
PRIVATE
|
||||
BVH.h
|
||||
BVH.cpp
|
||||
CollisionSystem.h
|
||||
CollisionSystem.cpp
|
||||
PhysicsSystem.h
|
||||
PhysicsSystem.cpp)
|
||||
|
||||
target_sources(Engine
|
||||
PUBLIC FILE_SET HEADERS
|
||||
FILES
|
||||
BVH.h
|
||||
CollisionSystem.h
|
||||
PhysicsSystem.h)
|
||||
@@ -0,0 +1,158 @@
|
||||
#include "CollisionSystem.h"
|
||||
|
||||
using namespace Seele;
|
||||
using namespace Seele::Component;
|
||||
|
||||
CollisionSystem::CollisionSystem()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
CollisionSystem::~CollisionSystem()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void CollisionSystem::detectCollisions(const entt::registry& registry, Array<Collision>& collisions)
|
||||
{
|
||||
collisions.clear();
|
||||
auto view = registry.view<Collider, Transform>();
|
||||
for(auto && [entity, collider, transform] : view.each())
|
||||
{
|
||||
if(collider.type == ColliderType::DYNAMIC && transform.isDirty())
|
||||
{
|
||||
bvh.updateDynamicCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
|
||||
}
|
||||
}
|
||||
Array<Pair<entt::entity, entt::entity>> overlaps;
|
||||
bvh.findOverlaps(overlaps);
|
||||
for(auto pair : overlaps)
|
||||
{
|
||||
if(checkCollision(registry, pair))
|
||||
{
|
||||
collisions.add(Collision {
|
||||
.a = pair.key,
|
||||
.b = pair.value,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool CollisionSystem::checkCollision(const entt::registry& registry, Pair<entt::entity, entt::entity> pair)
|
||||
{
|
||||
const auto&[collider1, transform1] = registry.get<Collider, Transform>(pair.key);
|
||||
const auto&[collider2, transform2] = registry.get<Collider, Transform>(pair.value);
|
||||
ShapeBase shape1 = collider1.physicsMesh.transform(transform1);
|
||||
ShapeBase shape2 = collider2.physicsMesh.transform(transform2);
|
||||
Witness witness;
|
||||
if(cachedWitness.exists(pair))
|
||||
{
|
||||
witness = cachedWitness[pair];
|
||||
}
|
||||
if(witnessValid(witness, shape1, shape2))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return createWitness(witness, shape1, shape2);
|
||||
}
|
||||
|
||||
void CollisionSystem::updateWitness(Witness& result, const glm::vec3& point, const glm::vec3& v1, const glm::vec3& v2)
|
||||
{
|
||||
const glm::vec3 faceNormal = glm::normalize(glm::cross(v1, v2));
|
||||
|
||||
result.n = faceNormal;
|
||||
result.p = point;
|
||||
}
|
||||
|
||||
|
||||
bool CollisionSystem::createWitness(Witness& result, const ShapeBase& source, const ShapeBase& other)
|
||||
{
|
||||
for (size_t i = 0; i < source.indices.size(); i += 3)
|
||||
{
|
||||
const glm::vec3 point1 = source.vertices[source.indices[i + 0]];
|
||||
const glm::vec3 point2 = source.vertices[source.indices[i + 1]];
|
||||
const glm::vec3 point3 = source.vertices[source.indices[i + 2]];
|
||||
const glm::vec3 v1 = point2 - point1;
|
||||
const glm::vec3 v2 = point3 - point1;
|
||||
|
||||
updateWitness(result, point1, v1, v2);
|
||||
result.point1Index = source.indices[i + 0];
|
||||
result.point2Index = source.indices[i + 1];
|
||||
result.point3Index = source.indices[i + 2];
|
||||
result.point4Index = UINT32_MAX;
|
||||
bool valid = witnessValid(result, source, other);
|
||||
// if not, it is a valid separating plane, so no collision
|
||||
if (valid)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (size_t i = 0; i < source.indices.size(); i += 3)
|
||||
{
|
||||
auto findEdgePlane = [=, &result](uint32_t point1Index, uint32_t point2Index)
|
||||
{
|
||||
const glm::vec3 point1 = source.vertices[point1Index];
|
||||
const glm::vec3 point2 = source.vertices[point2Index];
|
||||
result.point1Index = point1Index;
|
||||
result.point2Index = point2Index;
|
||||
const glm::vec3 d1 = point2 - point1;
|
||||
for (size_t j = 0; j < other.indices.size(); j += 3)
|
||||
{
|
||||
for (const auto& [point3Index, point4Index] : { std::pair(j+1, j), std::pair(j+2, j+1), std::pair(j, j+2) })
|
||||
{
|
||||
result.point3Index = other.indices[point3Index];
|
||||
result.point4Index = other.indices[point4Index];
|
||||
const glm::vec3 point3 = other.vertices[result.point3Index];
|
||||
const glm::vec3 point4 = other.vertices[result.point4Index];
|
||||
const glm::vec3 d2 = point3 - point4;
|
||||
updateWitness(result, point1, d2, d1);
|
||||
if (witnessValid(result, source, other))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
};
|
||||
if(findEdgePlane(source.indices[i], source.indices[i+1]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(findEdgePlane(source.indices[i+1], source.indices[i+2]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(findEdgePlane(source.indices[i+2], source.indices[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// no separating plane was found, collision
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CollisionSystem::witnessValid(const Witness& witness, const Component::ShapeBase& shape1, const Component::ShapeBase& shape2)
|
||||
{
|
||||
const float e = 0.0001f;
|
||||
|
||||
for (size_t i = 0; i < shape1.vertices.size(); i++)
|
||||
{
|
||||
if(glm::dot(witness.n, shape1.vertices[i] - witness.p) > e)
|
||||
{
|
||||
// something intersecting the separating plane
|
||||
// it is not valid anymore
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (size_t i = 0; i < shape2.vertices.size(); i++)
|
||||
{
|
||||
if(glm::dot(witness.n, shape2.vertices[i] - witness.p) < -e)
|
||||
{
|
||||
// something intersecting the separating plane
|
||||
// it is not valid anymore
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
#include <entt/entt.hpp>
|
||||
#include "BVH.h"
|
||||
#include "Containers/Map.h"
|
||||
#include "Containers/Array.h"
|
||||
#include "Component/Collider.h"
|
||||
#include "Component/Transform.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
struct Collision
|
||||
{
|
||||
entt::entity a, b;
|
||||
};
|
||||
class CollisionSystem
|
||||
{
|
||||
public:
|
||||
CollisionSystem();
|
||||
virtual ~CollisionSystem();
|
||||
void detectCollisions(const entt::registry& registry, Array<Collision>& collisions);
|
||||
private:
|
||||
struct Witness
|
||||
{
|
||||
Math::Vector p;
|
||||
Math::Vector n;
|
||||
// for finding p
|
||||
uint32_t point1Index;
|
||||
// and the face where the plane lies on
|
||||
uint32_t point2Index;
|
||||
uint32_t point3Index;
|
||||
uint32_t point4Index = UINT32_MAX;
|
||||
entt::entity source;
|
||||
entt::entity other;
|
||||
};
|
||||
BVH bvh;
|
||||
Map<Pair<entt::entity, entt::entity>, Witness> cachedWitness;
|
||||
|
||||
bool checkCollision(const entt::registry& registry, Pair<entt::entity, entt::entity> pair);
|
||||
|
||||
void updateWitness(Witness& witness, const Math::Vector& point, const Math::Vector& v1, const Math::Vector& v2);
|
||||
|
||||
// returns true if a collision was found
|
||||
bool createWitness(Witness& witness, const Component::ShapeBase& source, const Component::ShapeBase& other);
|
||||
|
||||
// returns true if a collision was found
|
||||
bool witnessValid(const Witness& witness, const Component::ShapeBase& shape1, const Component::ShapeBase& shape2);
|
||||
};
|
||||
} // namespace Seele
|
||||
@@ -0,0 +1,549 @@
|
||||
#include "PhysicsSystem.h"
|
||||
#include <boost/numeric/odeint.hpp>
|
||||
|
||||
using namespace Seele;
|
||||
using namespace Seele::Component;
|
||||
|
||||
PhysicsSystem::PhysicsSystem()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
PhysicsSystem::~PhysicsSystem()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void PhysicsSystem::update(entt::registry& registry, float deltaTime)
|
||||
{
|
||||
Array<Body> initialBodies;
|
||||
readRigidBodies(initialBodies, registry);
|
||||
|
||||
Array<Body> bodies = integratePhysics(initialBodies, 0, deltaTime);
|
||||
writeRigidBodies(bodies, registry);
|
||||
|
||||
Array<Collision> collisions;
|
||||
collisionSystem.detectCollisions(registry, collisions);
|
||||
|
||||
if(!collisions.empty())
|
||||
{
|
||||
constexpr size_t numSteps = 2;
|
||||
for (float t = 0; t < deltaTime; t += deltaTime / numSteps)
|
||||
{
|
||||
rewindCollisions(initialBodies, registry, t, t + (deltaTime / numSteps), 10);
|
||||
readRigidBodies(initialBodies, registry);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PhysicsSystem::serializeRB(const Body& rb, float* y) const
|
||||
{
|
||||
*y++ = rb.x.x;
|
||||
*y++ = rb.x.y;
|
||||
*y++ = rb.x.z;
|
||||
|
||||
*y++ = rb.q.w;
|
||||
*y++ = rb.q.x;
|
||||
*y++ = rb.q.y;
|
||||
*y++ = rb.q.z;
|
||||
|
||||
*y++ = rb.P.x;
|
||||
*y++ = rb.P.y;
|
||||
*y++ = rb.P.z;
|
||||
|
||||
*y++ = rb.L.x;
|
||||
*y++ = rb.L.y;
|
||||
*y++ = rb.L.z;
|
||||
}
|
||||
|
||||
void PhysicsSystem::deserializeRB(Body& rb, const float* y) const
|
||||
{
|
||||
rb.x.x = *y++;
|
||||
rb.x.y = *y++;
|
||||
rb.x.z = *y++;
|
||||
|
||||
rb.q.w = *y++;
|
||||
rb.q.x = *y++;
|
||||
rb.q.y = *y++;
|
||||
rb.q.z = *y++;
|
||||
|
||||
rb.P.x = *y++;
|
||||
rb.P.y = *y++;
|
||||
rb.P.z = *y++;
|
||||
|
||||
rb.L.x = *y++;
|
||||
rb.L.y = *y++;
|
||||
rb.L.z = *y++;
|
||||
|
||||
rb.v = rb.P * rb.inverseMass;
|
||||
rb.R = glm::mat3_cast(glm::normalize(rb.q));
|
||||
rb.iInv = rb.R * rb.iBodyInv * glm::transpose(rb.R);
|
||||
rb.omega = rb.iInv * rb.L;
|
||||
}
|
||||
|
||||
void PhysicsSystem::serializeArray(const Array<Body>& bodies, Array<float>& x) const
|
||||
{
|
||||
x.resize(bodies.size() * FLOATS_PER_RB);
|
||||
for(uint32_t i = 0; i < bodies.size(); ++i)
|
||||
{
|
||||
serializeRB(bodies[i], x.data()+(i*FLOATS_PER_RB));
|
||||
}
|
||||
}
|
||||
|
||||
void PhysicsSystem::deserializeArray(Array<Body>& bodies, const Array<float>& x) const
|
||||
{
|
||||
bodies.resize(x.size() / FLOATS_PER_RB);
|
||||
for(uint32_t i = 0; i < bodies.size(); ++i)
|
||||
{
|
||||
deserializeRB(bodies[i], x.data()+(i*FLOATS_PER_RB));
|
||||
}
|
||||
}
|
||||
|
||||
void PhysicsSystem::readRigidBodies(Array<Body>& bodies, entt::registry& registry) const
|
||||
{
|
||||
auto view = registry.view<RigidBody, Transform, Collider>();
|
||||
bodies.clear();
|
||||
bodies.reserve(view.size_hint());
|
||||
view
|
||||
.each([&bodies](entt::entity id, RigidBody& rb, Transform& transform, Collider& collider)
|
||||
{
|
||||
Body& rigidBody = bodies.add(Body(id, rb, collider, transform));
|
||||
rigidBody.updateMatrix();
|
||||
});
|
||||
registry.view<Collider, Transform>(entt::exclude<RigidBody>)
|
||||
.each([&bodies](entt::entity id, Collider& collider, Transform& transform)
|
||||
{
|
||||
Body& rigidBody = bodies.add(Body(id, collider, transform));
|
||||
rigidBody.updateMatrix();
|
||||
});
|
||||
}
|
||||
|
||||
void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies, entt::registry& registry) const
|
||||
{
|
||||
for(auto& body : bodies)
|
||||
{
|
||||
if(registry.all_of<RigidBody, Transform>(body.id))
|
||||
{
|
||||
auto [physics, transform] = registry.get<RigidBody, Transform>(body.id);
|
||||
transform.setPosition(body.x);
|
||||
transform.setRotation(body.q);
|
||||
physics.linearMomentum = body.P;
|
||||
physics.angularMomentum = body.L;
|
||||
physics.force = body.force;
|
||||
physics.torque = body.torque;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity, entt::registry& registry) const
|
||||
{
|
||||
Body rigidBody;
|
||||
if(registry.all_of<RigidBody, Collider, Transform>(entity))
|
||||
{
|
||||
const auto& [physics, collider, transform] = registry.get<RigidBody, Collider, Transform>(entity);
|
||||
rigidBody = Body(entity, physics, collider, transform);
|
||||
}
|
||||
else
|
||||
{
|
||||
const auto& [collider, transform] = registry.get<Collider, Transform>(entity);
|
||||
rigidBody = Body(entity, collider, transform);
|
||||
}
|
||||
rigidBody.updateMatrix();
|
||||
return rigidBody;
|
||||
}
|
||||
|
||||
|
||||
void PhysicsSystem::writeRigidBody(const Body& body, entt::registry& registry) const
|
||||
{
|
||||
if(registry.all_of<RigidBody, Transform>(body.id))
|
||||
{
|
||||
const auto& [physics, transform] = registry.get<RigidBody, Transform>(body.id);
|
||||
transform.setPosition(body.x);
|
||||
transform.setRotation(body.q);
|
||||
physics.linearMomentum = body.P;
|
||||
physics.angularMomentum = body.L;
|
||||
physics.force = body.force;
|
||||
physics.torque = body.torque;
|
||||
}
|
||||
else
|
||||
{
|
||||
auto& transform = registry.get<Transform>(body.id);
|
||||
assert(transform.getPosition() == body.x);
|
||||
assert(transform.getRotation() == body.q);
|
||||
}
|
||||
}
|
||||
|
||||
Array<PhysicsSystem::Body> PhysicsSystem::integratePhysics(const Array<Body>& bodies, const float t0, const float tdelta) const
|
||||
{
|
||||
Array<Body> result;
|
||||
Array<float> buffer;
|
||||
result.resize(bodies.size());
|
||||
buffer.resize(bodies.size() * FLOATS_PER_RB);
|
||||
std::memcpy(result.data(), bodies.data(), result.size() * sizeof(Body));
|
||||
serializeArray(bodies, buffer);
|
||||
auto dxdt = [this, &result](const Array<float>& x, Array<float>& x2, const float)
|
||||
{
|
||||
deserializeArray(result, x);
|
||||
float* xdot = x2.data();
|
||||
for (size_t i = 0; i < result.size(); i++)
|
||||
{
|
||||
// x(t)' = v(t)
|
||||
*xdot++ = result[i].v.x;
|
||||
*xdot++ = result[i].v.y;
|
||||
*xdot++ = result[i].v.z;
|
||||
|
||||
// R(t)' = omega(t)*R(t)
|
||||
Math::Quaternion qdot = 0.5f * (Math::Quaternion(0, result[i].omega) * result[i].q);
|
||||
*xdot++ = qdot.w;
|
||||
*xdot++ = qdot.x;
|
||||
*xdot++ = qdot.y;
|
||||
*xdot++ = qdot.z;
|
||||
|
||||
// P(t)' = F(t)
|
||||
*xdot++ = result[i].force.x;
|
||||
*xdot++ = result[i].force.y;
|
||||
*xdot++ = result[i].force.z;
|
||||
|
||||
// L(t)' = tau(t)
|
||||
*xdot++ = result[i].torque.x;
|
||||
*xdot++ = result[i].torque.y;
|
||||
*xdot++ = result[i].torque.z;
|
||||
}
|
||||
};
|
||||
boost::numeric::odeint::stepper_rk4<Array<float>, float> stepper;
|
||||
boost::numeric::odeint::integrate_const(stepper, dxdt, buffer, t0, tdelta, tdelta);
|
||||
deserializeArray(result, buffer);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, entt::registry& registry, const float t0, const float t1, size_t remainingRecursionDepth)
|
||||
{
|
||||
if(remainingRecursionDepth == 0)
|
||||
{
|
||||
//std::cout << "reached max recursion depth" << std::endl;
|
||||
}
|
||||
// there are collisions happening between t0 and t1
|
||||
// we integrate until tc and see if they have already occured then
|
||||
Array<Collision> collisions;
|
||||
writeRigidBodies(integratePhysics(t0Bodies, t0, t1), registry);
|
||||
collisionSystem.detectCollisions(registry, collisions);
|
||||
|
||||
//std::cout << "detected " << collisions.size() << " at " << tc << std::endl;
|
||||
// now we check if there has been a contact at tc
|
||||
Array<Contact> contacts;
|
||||
// collision occured at [tc; t1]
|
||||
for (auto &&collision : collisions)
|
||||
{
|
||||
const auto&[collider1, transform1] = registry.get<Collider, Transform>(collision.a);
|
||||
const auto&[collider2, transform2] = registry.get<Collider, Transform>(collision.b);
|
||||
calculateContacts(collision.a, collider1.physicsMesh.transform(transform1), collision.b, collider2.physicsMesh.transform(transform2), contacts);
|
||||
}
|
||||
|
||||
// we then apply forces in order to counteract interpenetration
|
||||
Array<Contact> restingContacts;
|
||||
for(const auto& contact : contacts)
|
||||
{
|
||||
Body a = readRigidBody(contact.a, registry);
|
||||
Body b = readRigidBody(contact.b, registry);
|
||||
Math::Vector paDot = a.ptVelocity(contact.p);
|
||||
Math::Vector pbDot = b.ptVelocity(contact.p);
|
||||
float vrel = glm::dot(contact.n, paDot - pbDot);
|
||||
if(vrel > 0.001f)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if(vrel > -0.001f)
|
||||
{
|
||||
restingContacts.add(contact);
|
||||
continue;
|
||||
}
|
||||
resolvePenetratingContact(contact, a, b);
|
||||
a.updateMatrix();
|
||||
b.updateMatrix();
|
||||
writeRigidBody(a, registry);
|
||||
writeRigidBody(b, registry);
|
||||
}
|
||||
resolveRestingContacts(restingContacts, registry);
|
||||
}
|
||||
|
||||
void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1, entt::entity id2, const ShapeBase& shape2, Array<Contact>& contacts) const
|
||||
{
|
||||
for(size_t i = 0; i < shape1.indices.size(); i += 3)
|
||||
{
|
||||
// face - vertex contacts
|
||||
const Math::Vector point1 = shape1.vertices[shape1.indices[i + 0]];
|
||||
const Math::Vector point2 = shape1.vertices[shape1.indices[i + 1]];
|
||||
const Math::Vector point3 = shape1.vertices[shape1.indices[i + 2]];
|
||||
const Math::Vector v1 = point2 - point1;
|
||||
const Math::Vector v2 = point3 - point1;
|
||||
const Math::Vector faceNormal = glm::normalize(glm::cross(v1, v2));
|
||||
auto area = [](Math::Vector ab, Math::Vector ac){
|
||||
return glm::length(glm::cross(ab, ac)) / 2.0f;
|
||||
};
|
||||
float faceArea = area(v1, v2);
|
||||
for(size_t j = 0; j < shape2.vertices.size(); j++)
|
||||
{
|
||||
Math::Vector worldPos = shape2.vertices[j];
|
||||
float dot = glm::dot(faceNormal, worldPos - point1);
|
||||
if(dot < 0.2f)
|
||||
{
|
||||
Math::Vector pa = point1 - worldPos;
|
||||
Math::Vector pb = point2 - worldPos;
|
||||
Math::Vector pc = point3 - worldPos;
|
||||
float a1 = area(pa, pb);
|
||||
float a2 = area(pb, pc);
|
||||
float a3 = area(pc, pa);
|
||||
|
||||
if(std::abs(a1 + a2 + a3 - faceArea) > 0.2f)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
Contact c = {
|
||||
.a = id2,
|
||||
.b = id1,
|
||||
.p = worldPos,
|
||||
.n = faceNormal,
|
||||
.vf = true
|
||||
};
|
||||
contacts.add(c);
|
||||
}
|
||||
}
|
||||
|
||||
// edge - edge contacts
|
||||
auto lineLineContact = [=, &contacts](Math::Vector p1, Math::Vector p2, Math::Vector p3, Math::Vector p4)
|
||||
{
|
||||
//L1 = p1 + t * p2 - p1;
|
||||
//L2 = p3 + u * p4 - p3;
|
||||
Math::Vector a = p1;
|
||||
Math::Vector c = p3;
|
||||
Math::Vector ab = p2 - p1;
|
||||
Math::Vector cd = p4 - p3;
|
||||
float tx_den = cd.z * ab.y - cd.y * ab.z;
|
||||
float tx = (c.y * ab.z - a.y * ab.z - c.z * ab.y + a.z * ab.y) / tx_den;
|
||||
float ty_den = cd.z * ab.x - cd.z * ab.z;
|
||||
float ty = (c.x * ab.z - a.x * ab.z - c.z * ab.x + a.z * ab.x) / ty_den;
|
||||
float tz_den = cd.y * ab.x - cd.x * ab.y;
|
||||
float tz = (c.x * ab.y - a.x * ab.y - c.y * ab.x + a.y * ab.x) / tz_den;
|
||||
if (std::abs(tx - ty) < 0.1f
|
||||
&& std::abs(ty - tz) < 0.1f
|
||||
&& std::abs(tz - tx) < 0.1f
|
||||
&& tx >= 0.f
|
||||
&& tx <= 1.f)
|
||||
{
|
||||
Math::Vector p = p1 + tx * (p2 - p1);
|
||||
Math::Vector ea = p2 - p1;
|
||||
Math::Vector eb = p4 - p3;
|
||||
Math::Vector n = glm::normalize(glm::cross(eb, ea));
|
||||
Contact contact = {
|
||||
.a = id1,
|
||||
.b = id2,
|
||||
.p = p,
|
||||
.n = n,
|
||||
.ea = ea,
|
||||
.eb = eb,
|
||||
.vf = false
|
||||
};
|
||||
}
|
||||
};
|
||||
for(size_t j = 0; j < shape2.indices.size(); j+=3)
|
||||
{
|
||||
const Math::Vector point4 = shape2.vertices[shape2.indices[j + 0]];
|
||||
const Math::Vector point5 = shape2.vertices[shape2.indices[j + 1]];
|
||||
const Math::Vector point6 = shape2.vertices[shape2.indices[j + 2]];
|
||||
|
||||
lineLineContact(point1, point2, point4, point5);
|
||||
lineLineContact(point1, point2, point5, point6);
|
||||
lineLineContact(point1, point2, point6, point4);
|
||||
|
||||
lineLineContact(point2, point3, point4, point5);
|
||||
lineLineContact(point2, point3, point5, point6);
|
||||
lineLineContact(point2, point3, point6, point4);
|
||||
|
||||
lineLineContact(point3, point1, point4, point5);
|
||||
lineLineContact(point3, point1, point5, point6);
|
||||
lineLineContact(point3, point1, point6, point4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts, entt::registry& registry) const
|
||||
{
|
||||
Array<Array<float>> amat(contacts.size(), Array<float>(contacts.size()));
|
||||
Array<float> bvec(contacts.size());
|
||||
|
||||
computeAMatrix(contacts, amat, registry);
|
||||
computeBVector(contacts, bvec, registry);
|
||||
|
||||
/*CGAL::Quadratic_program<float> qp(CGAL::SMALLER, true, 0, false, 0);
|
||||
for(size_t x = 0; x < contacts.size(); ++x)
|
||||
{
|
||||
for(size_t y = 0; y < contacts.size(); ++y)
|
||||
{
|
||||
qp.set_a(x, y, amat[x][y]);
|
||||
}
|
||||
}
|
||||
for(size_t y = 0; y < contacts.size(); ++y)
|
||||
{
|
||||
qp.set_b(y, bvec[y]);
|
||||
}
|
||||
CGAL::Quadratic_program_solution<CGAL::MP_Float> s = CGAL::solve_quadratic_program(qp, CGAL::MP_Float());
|
||||
assert(s.solves_quadratic_program(qp));
|
||||
|
||||
auto solutionBegin = s.variable_values_begin();
|
||||
for(size_t y = 0; y < contacts.size(); ++y)
|
||||
{
|
||||
float f = CGAL::to_double(*solutionBegin++);
|
||||
Math::Vector n = contacts[y].n;
|
||||
RigidBody a = readRigidBody(contacts[y].a, registry);
|
||||
RigidBody b = readRigidBody(contacts[y].b, registry);
|
||||
a.force += f * n;
|
||||
a.torque += (contacts[y].p - a.x + a.centerOfMass) * (f * n);
|
||||
|
||||
b.force -= f * n;
|
||||
b.torque -= (contacts[y].p - b.x + b.centerOfMass) * (f * n);
|
||||
writeRigidBody(a, registry);
|
||||
writeRigidBody(b, registry);
|
||||
}*/
|
||||
}
|
||||
|
||||
void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, Body& b) const
|
||||
{
|
||||
Math::Vector paDot = a.ptVelocity(contact.p);
|
||||
Math::Vector pbDot = b.ptVelocity(contact.p);
|
||||
float vrel = glm::dot(contact.n, paDot - pbDot);
|
||||
Math::Vector n = contact.n;
|
||||
Math::Vector ra = contact.p - a.x + a.centerOfMass;
|
||||
Math::Vector rb = contact.p - b.x + b.centerOfMass;
|
||||
float numerator = -(1 + 0.5f) * vrel;
|
||||
|
||||
float term1 = a.inverseMass;
|
||||
float term2 = b.inverseMass;
|
||||
float term3 = glm::dot(n, glm::cross(a.iInv * glm::cross(ra, n), ra));
|
||||
float term4 = glm::dot(n, glm::cross(b.iInv * glm::cross(rb, n), rb));
|
||||
|
||||
float j = numerator / (term1 + term2 + term3 + term4);
|
||||
Math::Vector force = j * n;
|
||||
|
||||
a.P += force;
|
||||
b.P -= force;
|
||||
a.L += glm::cross(ra, force);
|
||||
b.L -= glm::cross(rb, force);
|
||||
|
||||
a.v = a.P * a.inverseMass;
|
||||
b.v = b.P * b.inverseMass;
|
||||
|
||||
a.omega = a.iInv * a.L;
|
||||
b.omega = b.iInv * b.L;
|
||||
}
|
||||
|
||||
Math::Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b) const
|
||||
{
|
||||
if(c.vf)
|
||||
{
|
||||
return glm::cross(b.omega, c.n);
|
||||
}
|
||||
else
|
||||
{
|
||||
Math::Vector eadot = glm::cross(a.omega, c.ea);
|
||||
Math::Vector ebdot = glm::cross(b.omega, c.eb);
|
||||
Math::Vector n1 = glm::cross(c.ea, c.eb);
|
||||
Math::Vector z = glm::cross(eadot, c.eb) + glm::cross(c.ea, ebdot);
|
||||
float l = glm::length(n1);
|
||||
n1 = glm::normalize(n1);
|
||||
|
||||
return (z - glm::cross(glm::cross(z, n1), n1)) / l;
|
||||
}
|
||||
}
|
||||
|
||||
float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj, entt::registry& registry) const
|
||||
{
|
||||
if((ci.a != cj.a) && (ci.b != cj.b) &&
|
||||
(ci.a != cj.b) && (ci.b != cj.a))
|
||||
return 0.0f;
|
||||
|
||||
Body a = readRigidBody(ci.a, registry);
|
||||
Body b = readRigidBody(ci.b, registry);
|
||||
Math::Vector ni = ci.n;
|
||||
Math::Vector nj = cj.n;
|
||||
Math::Vector pi = ci.p;
|
||||
Math::Vector pj = cj.p;
|
||||
Math::Vector ra = pi - a.x + a.centerOfMass;
|
||||
Math::Vector rb = pi - b.x + b.centerOfMass;
|
||||
|
||||
Math::Vector forceOnA = Math::Vector(0);
|
||||
Math::Vector torqueOnA = Math::Vector(0);
|
||||
if(cj.a == ci.a)
|
||||
{
|
||||
forceOnA = nj;
|
||||
torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
|
||||
}
|
||||
else if(cj.b == ci.a)
|
||||
{
|
||||
forceOnA = -nj;
|
||||
torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
|
||||
}
|
||||
Math::Vector forceOnB = Math::Vector(0);
|
||||
Math::Vector torqueOnB = Math::Vector(0);
|
||||
if(cj.a == ci.b)
|
||||
{
|
||||
forceOnB = nj;
|
||||
torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
|
||||
}
|
||||
else if(cj.b == ci.b)
|
||||
{
|
||||
forceOnB = -nj;
|
||||
torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
|
||||
}
|
||||
|
||||
Math::Vector aLinear = forceOnA * a.inverseMass;
|
||||
Math::Vector aAngular = glm::cross((a.iInv * torqueOnA), ra);
|
||||
|
||||
Math::Vector bLinear = forceOnB * b.inverseMass;
|
||||
Math::Vector bAngular = glm::cross((b.iInv * torqueOnB), rb);
|
||||
|
||||
return glm::dot(ni, ((aLinear + aAngular) - (bLinear + bAngular)));
|
||||
}
|
||||
|
||||
void PhysicsSystem::computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat, entt::registry& registry) const
|
||||
{
|
||||
for(size_t x = 0; x < contacts.size(); ++x)
|
||||
{
|
||||
for(size_t y = 0; y < contacts.size(); ++y)
|
||||
{
|
||||
amat[x][y] = computeAij(contacts[x], contacts[y], registry);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>& bvec, entt::registry& registry) const
|
||||
{
|
||||
for(size_t y = 0; y < contacts.size(); ++y)
|
||||
{
|
||||
Contact c = contacts[y];
|
||||
Body a = readRigidBody(c.a, registry);
|
||||
Body b = readRigidBody(c.b, registry);
|
||||
Math::Vector n = c.n;
|
||||
Math::Vector ra = c.p - a.x + a.centerOfMass;
|
||||
Math::Vector rb = c.p - b.x + b.centerOfMass;
|
||||
|
||||
Math::Vector fExtA = a.force;
|
||||
Math::Vector fExtB = b.force;
|
||||
Math::Vector tExtA = a.torque;
|
||||
Math::Vector tExtB = b.torque;
|
||||
|
||||
Math::Vector aExtPart = fExtA * a.inverseMass + glm::cross(a.iInv * tExtA, ra);
|
||||
Math::Vector bExtPart = fExtB * b.inverseMass + glm::cross(b.iInv * tExtB, rb);
|
||||
|
||||
Math::Vector aVelPart = glm::cross(a.omega, glm::cross(a.omega, ra)) + glm::cross(a.iInv * glm::cross(a.L, a.omega), ra);
|
||||
Math::Vector bVelPart = glm::cross(b.omega, glm::cross(b.omega, rb)) + glm::cross(b.iInv * glm::cross(b.L, b.omega), rb);
|
||||
|
||||
float k1 = glm::dot(n, (aExtPart + aVelPart) - (bExtPart + bVelPart));
|
||||
Math::Vector ndot = computeNdot(c, a, b);
|
||||
|
||||
float k2 = 2.0f * glm::dot(ndot, (a.ptVelocity(c.p) - b.ptVelocity(c.p)));
|
||||
bvec[y] = k1 + k2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
#pragma once
|
||||
#include <entt/entt.hpp>
|
||||
#include "MinimalEngine.h"
|
||||
#include "Component/Transform.h"
|
||||
#include "Component/RigidBody.h"
|
||||
#include "Component/Collider.h"
|
||||
#include "CollisionSystem.h"
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
class PhysicsSystem
|
||||
{
|
||||
public:
|
||||
PhysicsSystem();
|
||||
~PhysicsSystem();
|
||||
void update(entt::registry& registry, float deltaTime);
|
||||
private:
|
||||
struct Body
|
||||
{
|
||||
entt::entity id;
|
||||
float inverseMass;
|
||||
Math::Vector centerOfMass;
|
||||
Math::Matrix3 iBody, iBodyInv;
|
||||
Math::Vector scale;
|
||||
|
||||
Math::Vector x;
|
||||
Math::Quaternion q;
|
||||
Math::Vector P;
|
||||
Math::Vector L;
|
||||
|
||||
Math::Matrix3 iInv;
|
||||
Math::Matrix3 R;
|
||||
Math::Vector v;
|
||||
Math::Vector omega;
|
||||
Math::Vector force;
|
||||
Math::Vector torque;
|
||||
Math::Matrix4 matrix;
|
||||
Math::Vector ptVelocity(Math::Vector p) const {return v + glm::cross(omega, p - x + centerOfMass);}
|
||||
void updateMatrix() {
|
||||
Math::Matrix4 scaleMatrix = glm::scale(Math::Matrix4(1), scale);
|
||||
Math::Matrix4 rotationMatrix = glm::mat4_cast(q);
|
||||
Math::Matrix4 translationMatrix = glm::translate(Math::Matrix4(1), x);
|
||||
matrix = translationMatrix * rotationMatrix * scaleMatrix;
|
||||
}
|
||||
Body()
|
||||
{}
|
||||
Body(entt::entity id, const Component::RigidBody& physics, const Component::Collider& collider, const Component::Transform& transform)
|
||||
: id(id)
|
||||
, inverseMass(1 / (physics.mass * glm::length(transform.getScale())))
|
||||
, centerOfMass(collider.physicsMesh.centerOfMass)
|
||||
, iBody(collider.physicsMesh.bodyInertia)
|
||||
, iBodyInv(glm::inverse(collider.physicsMesh.bodyInertia))
|
||||
, scale(transform.getScale())
|
||||
, x(transform.getPosition())
|
||||
, q(transform.getRotation())
|
||||
, P(physics.linearMomentum)
|
||||
, L(physics.angularMomentum)
|
||||
, iInv(glm::mat3())
|
||||
, R(glm::mat3())
|
||||
, v(Math::Vector())
|
||||
, omega(Math::Vector())
|
||||
, force(physics.force)
|
||||
, torque(physics.torque)
|
||||
{
|
||||
v = P * inverseMass;
|
||||
R = glm::mat3_cast(glm::normalize(q));
|
||||
iInv = R * iBodyInv * glm::transpose(R);
|
||||
omega = iInv * L;
|
||||
}
|
||||
Body(entt::entity id, const Component::Collider& collider, const Component::Transform& transform)
|
||||
: id(id)
|
||||
, inverseMass(0)
|
||||
, centerOfMass(collider.physicsMesh.centerOfMass)
|
||||
, iBody(glm::mat3(0))
|
||||
, iBodyInv(glm::mat3(0))
|
||||
, scale(transform.getScale())
|
||||
, x(transform.getPosition())
|
||||
, q(transform.getRotation())
|
||||
, P(Math::Vector(0))
|
||||
, L(Math::Vector(0))
|
||||
, iInv(glm::mat3())
|
||||
, R(glm::mat3())
|
||||
, v(Math::Vector())
|
||||
, omega(Math::Vector())
|
||||
, force(Math::Vector(0))
|
||||
, torque(Math::Vector(0))
|
||||
{
|
||||
R = glm::mat3_cast(glm::normalize(q));
|
||||
iInv = R * iBodyInv * glm::transpose(R);
|
||||
omega = iInv * L;
|
||||
}
|
||||
};
|
||||
struct Contact
|
||||
{
|
||||
entt::entity a, b;
|
||||
glm::vec3 p;
|
||||
glm::vec3 n;
|
||||
glm::vec3 ea;
|
||||
glm::vec3 eb;
|
||||
bool vf;
|
||||
};
|
||||
static constexpr size_t FLOATS_PER_RB = sizeof(Body) / sizeof(float);
|
||||
|
||||
CollisionSystem collisionSystem;
|
||||
|
||||
void serializeRB(const Body& rb, float* y) const;
|
||||
|
||||
void deserializeRB(Body& rb, const float* y) const;
|
||||
|
||||
void serializeArray(const Array<Body>& bodies, Array<float>& x) const;
|
||||
|
||||
void deserializeArray(Array<Body>& bodies, const Array<float>& x) const;
|
||||
|
||||
void readRigidBodies(Array<Body>& bodies, entt::registry& registry) const;
|
||||
|
||||
void writeRigidBodies(const Array<Body>& bodies, entt::registry& registry) const;
|
||||
|
||||
Body readRigidBody(entt::entity entity, entt::registry& regsitry) const;
|
||||
|
||||
void writeRigidBody(const Body& body, entt::registry& registry) const;
|
||||
|
||||
Array<Body> integratePhysics(const Array<Body>& bodies, const float t0, const float tdelta) const;
|
||||
|
||||
void rewindCollisions(const Array<Body>& t0Bodies, entt::registry& registry, const float t0, const float t1, size_t remainingDepth);
|
||||
|
||||
void calculateContacts(entt::entity id1, const Component::ShapeBase& shape1, entt::entity id2, const Component::ShapeBase& shape2, Array<Contact>& contacts) const;
|
||||
|
||||
void resolveRestingContacts(const Array<Contact>& contacts, entt::registry& registry) const;
|
||||
|
||||
void resolvePenetratingContact(const Contact& c, Body& a, Body& b) const;
|
||||
|
||||
Math::Vector computeNdot(const Contact& c, const Body& a, const Body& b) const;
|
||||
|
||||
float computeAij(const Contact& ci, const Contact& cj, entt::registry& registry) const;
|
||||
|
||||
void computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat, entt::registry& registry) const;
|
||||
|
||||
void computeBVector(const Array<Contact>& contacts, Array<float>& avec, entt::registry& registry) const;
|
||||
};
|
||||
} // namespace Seele
|
||||
@@ -1,5 +1,6 @@
|
||||
target_sources(Engine
|
||||
PRIVATE
|
||||
EventManager.h
|
||||
Util.h
|
||||
Scene.cpp
|
||||
Scene.h)
|
||||
@@ -7,5 +8,6 @@ target_sources(Engine
|
||||
target_sources(Engine
|
||||
PUBLIC FILE_SET HEADERS
|
||||
FILES
|
||||
EventManager.h
|
||||
Util.h
|
||||
Scene.h)
|
||||
@@ -0,0 +1,25 @@
|
||||
#pragma once
|
||||
#include <entt/entt.hpp>
|
||||
|
||||
namespace Seele
|
||||
{
|
||||
template<typename T>
|
||||
struct Event
|
||||
{
|
||||
};
|
||||
class EventManager
|
||||
{
|
||||
public:
|
||||
template<typename T>
|
||||
void pushEvent(Event<T> event)
|
||||
{
|
||||
|
||||
}
|
||||
template<typename T>
|
||||
void subscribe()
|
||||
{
|
||||
|
||||
}
|
||||
private:
|
||||
};
|
||||
} // namespace Seele
|
||||
@@ -14,11 +14,16 @@ class SystemBase
|
||||
public:
|
||||
SystemBase(entt::registry& registry) : registry(registry) {}
|
||||
virtual ~SystemBase() {}
|
||||
template<typename Comp>
|
||||
template<is_component Comp>
|
||||
auto getDependencies()
|
||||
{
|
||||
return Comp::dependencies;
|
||||
}
|
||||
template<typename Comp>
|
||||
Dependencies<> getDependencies()
|
||||
{
|
||||
return Dependencies<>();
|
||||
}
|
||||
template<typename... Dep>
|
||||
auto mergeDependencies(Dep... deps)
|
||||
{
|
||||
@@ -29,17 +34,28 @@ public:
|
||||
{
|
||||
registry.view<Components..., Deps...>().each([&](Components&... comp, Deps&... deps){
|
||||
//pool.enqueue_detach([&](){
|
||||
int ret = (accessComponent(deps) + ...);
|
||||
assert(ret == 0);
|
||||
(accessComponent(deps) + ...);
|
||||
update((comp,...));
|
||||
//});
|
||||
});
|
||||
}
|
||||
void run(dp::thread_pool<>& pool)
|
||||
template<>
|
||||
void setupView(Dependencies<>, dp::thread_pool<>&)
|
||||
{
|
||||
registry.view<Components...>().each([&](Components&... comp){
|
||||
//pool.enqueue_detach([&](){
|
||||
update(comp...);
|
||||
//});
|
||||
});
|
||||
}
|
||||
void run(dp::thread_pool<>& pool, double delta)
|
||||
{
|
||||
deltaTime = delta;
|
||||
setupView(mergeDependencies((getDependencies<Components>(),...)), pool);
|
||||
}
|
||||
virtual void update(Components&... components) = 0;
|
||||
protected:
|
||||
double deltaTime;
|
||||
private:
|
||||
entt::registry& registry;
|
||||
};
|
||||
|
||||
@@ -10,18 +10,6 @@ WindowManager::WindowManager()
|
||||
graphics = new Vulkan::Graphics();
|
||||
GraphicsInitializer initializer;
|
||||
graphics->init(initializer);
|
||||
TextureCreateInfo info;
|
||||
info.width = 4096;
|
||||
info.height = 4096;
|
||||
Gfx::PTexture2D testTexture = graphics->createTexture2D(info);
|
||||
UniformBufferCreateInfo uniformInitializer;
|
||||
uniformInitializer.resourceData.size = 4096;
|
||||
uniformInitializer.resourceData.data = new uint8[4096];
|
||||
for (int i = 0; i < 4096; ++i)
|
||||
{
|
||||
uniformInitializer.resourceData.data[i] = (uint8)i;
|
||||
}
|
||||
Gfx::PUniformBuffer testUniform = graphics->createUniformBuffer(uniformInitializer);
|
||||
}
|
||||
|
||||
WindowManager::~WindowManager()
|
||||
|
||||
Reference in New Issue
Block a user