luboslenco
347 lines
11 KiB
C++
347 lines
11 KiB
C++
|
|
#include "phys_jolt.h"
|
|
#include <Jolt/Jolt.h>
|
|
#include <Jolt/RegisterTypes.h>
|
|
#include <Jolt/Core/Factory.h>
|
|
#include <Jolt/Core/TempAllocator.h>
|
|
#include <Jolt/Core/JobSystemThreadPool.h>
|
|
#include <Jolt/Physics/PhysicsSettings.h>
|
|
#include <Jolt/Physics/PhysicsSystem.h>
|
|
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/MeshShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
|
|
#include <Jolt/Physics/Body/BodyCreationSettings.h>
|
|
#include <Jolt/Physics/Body/MotionQuality.h>
|
|
#include <kinc/math/vector.h>
|
|
#include <kinc/math/quaternion.h>
|
|
#include "iron_array.h"
|
|
|
|
JPH_SUPPRESS_WARNINGS
|
|
using namespace JPH;
|
|
using namespace JPH::literals;
|
|
|
|
PhysicsSystem *physics_system;
|
|
TempAllocatorImpl *temp_allocator;
|
|
JobSystemThreadPool *job_system;
|
|
physics_pair_t ppair;
|
|
|
|
static void TraceImpl(const char *inFMT, ...) {
|
|
}
|
|
|
|
namespace Layers {
|
|
static constexpr ObjectLayer NON_MOVING = 0;
|
|
static constexpr ObjectLayer MOVING = 1;
|
|
static constexpr ObjectLayer NUM_LAYERS = 2;
|
|
};
|
|
|
|
class ObjectLayerPairFilterImpl : public ObjectLayerPairFilter {
|
|
public:
|
|
virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override {
|
|
switch (inObject1) {
|
|
case Layers::NON_MOVING:
|
|
return inObject2 == Layers::MOVING;
|
|
case Layers::MOVING:
|
|
return true;
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
};
|
|
|
|
namespace BroadPhaseLayers {
|
|
static constexpr BroadPhaseLayer NON_MOVING(0);
|
|
static constexpr BroadPhaseLayer MOVING(1);
|
|
static constexpr uint NUM_LAYERS(2);
|
|
};
|
|
|
|
class BPLayerInterfaceImpl final : public BroadPhaseLayerInterface {
|
|
public:
|
|
BPLayerInterfaceImpl() {
|
|
mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
|
|
mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
|
|
}
|
|
|
|
virtual uint GetNumBroadPhaseLayers() const override {
|
|
return BroadPhaseLayers::NUM_LAYERS;
|
|
}
|
|
|
|
virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override {
|
|
return mObjectToBroadPhase[inLayer];
|
|
}
|
|
|
|
#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
|
|
virtual const char *GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override {
|
|
return "";
|
|
}
|
|
#endif
|
|
|
|
BroadPhaseLayer mObjectToBroadPhase[Layers::NUM_LAYERS];
|
|
};
|
|
|
|
class ObjectVsBroadPhaseLayerFilterImpl : public ObjectVsBroadPhaseLayerFilter {
|
|
public:
|
|
virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override {
|
|
switch (inLayer1) {
|
|
case Layers::NON_MOVING:
|
|
return inLayer2 == BroadPhaseLayers::MOVING;
|
|
case Layers::MOVING:
|
|
return true;
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
};
|
|
|
|
class MyContactListener : public ContactListener {
|
|
public:
|
|
virtual ValidateResult OnContactValidate(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) override {
|
|
return ValidateResult::AcceptAllContactsForThisBodyPair;
|
|
}
|
|
|
|
virtual void OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override {
|
|
}
|
|
|
|
virtual void OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override {
|
|
ppair.pos_a_x = inManifold.GetWorldSpaceContactPointOn1(0).GetX();
|
|
ppair.pos_a_y = inManifold.GetWorldSpaceContactPointOn1(0).GetY();
|
|
ppair.pos_a_z = inManifold.GetWorldSpaceContactPointOn1(0).GetZ();
|
|
}
|
|
|
|
virtual void OnContactRemoved(const SubShapeIDPair &inSubShapePair) override {
|
|
ppair.pos_a_x = ppair.pos_a_y = ppair.pos_a_z = 0;
|
|
}
|
|
};
|
|
|
|
typedef enum {
|
|
physics_shape_BOX = 0,
|
|
physics_shape_SPHERE = 1,
|
|
physics_shape_HULL = 2,
|
|
physics_shape_TERRAIN = 3,
|
|
physics_shape_MESH = 4,
|
|
} physics_shape_t;
|
|
|
|
BPLayerInterfaceImpl broad_phase_layer_interface;
|
|
ObjectVsBroadPhaseLayerFilterImpl object_vs_broadphase_layer_filter;
|
|
ObjectLayerPairFilterImpl object_vs_object_layer_filter;
|
|
|
|
void _jolt_world_create() {
|
|
RegisterDefaultAllocator();
|
|
Trace = TraceImpl;
|
|
Factory::sInstance = new Factory();
|
|
RegisterTypes();
|
|
|
|
temp_allocator = new TempAllocatorImpl(10 * 1024 * 1024);
|
|
// job_system = new JobSystemThreadPool(cMaxPhysicsJobs, cMaxPhysicsBarriers, thread::hardware_concurrency() - 1);
|
|
job_system = new JobSystemThreadPool(cMaxPhysicsJobs, cMaxPhysicsBarriers, 0);
|
|
|
|
const uint cMaxBodies = 1024;
|
|
const uint cNumBodyMutexes = 0;
|
|
const uint cMaxBodyPairs = 1024;
|
|
const uint cMaxContactConstraints = 1024;
|
|
|
|
physics_system = new PhysicsSystem();
|
|
physics_system->Init(cMaxBodies, cNumBodyMutexes, cMaxBodyPairs, cMaxContactConstraints,
|
|
broad_phase_layer_interface, object_vs_broadphase_layer_filter, object_vs_object_layer_filter);
|
|
|
|
MyContactListener *contact_listener = new MyContactListener();
|
|
physics_system->SetContactListener(contact_listener);
|
|
|
|
physics_system->SetGravity(Vec3(0.0, 0.0, -9.81));
|
|
}
|
|
|
|
void _jolt_world_update() {
|
|
#ifdef is_forge
|
|
const int cCollisionSteps = 2;
|
|
#else
|
|
const int cCollisionSteps = 1;
|
|
#endif
|
|
const float cDeltaTime = 1.0f / 60.0f;
|
|
physics_system->Update(cDeltaTime, cCollisionSteps, temp_allocator, job_system);
|
|
}
|
|
|
|
void _jolt_world_destroy() {
|
|
// body_interface.RemoveBody(sphere_id);
|
|
// body_interface.DestroyBody(sphere_id);
|
|
// body_interface.RemoveBody(floor->GetID());
|
|
// body_interface.DestroyBody(floor->GetID());
|
|
UnregisterTypes();
|
|
delete Factory::sInstance;
|
|
Factory::sInstance = nullptr;
|
|
}
|
|
|
|
void *_jolt_body_create(int shape, float mass, float dimx, float dimy, float dimz, float x, float y, float z, void *f32a_triangles) {
|
|
BodyInterface &body_interface = physics_system->GetBodyInterface();
|
|
Body *body;
|
|
ShapeSettings::ShapeResult result;
|
|
|
|
// float convex_radius = 0.05f;
|
|
float convex_radius = 0.0f;
|
|
|
|
// if (convex_radius > 0.0) {
|
|
// if (dimx <= 0.2) dimx = 0.21;
|
|
// if (dimy <= 0.2) dimy = 0.21;
|
|
// if (dimz <= 0.2) dimz = 0.21;
|
|
// }
|
|
|
|
if (shape == physics_shape_BOX) {
|
|
BoxShapeSettings shape_settings(RVec3(dimx / 2.0, dimy / 2.0, dimz / 2.0), convex_radius);
|
|
shape_settings.SetEmbedded();
|
|
result = shape_settings.Create();
|
|
}
|
|
else if (shape == physics_shape_SPHERE) {
|
|
SphereShapeSettings shape_settings(dimx / 2.0);
|
|
shape_settings.SetEmbedded();
|
|
result = shape_settings.Create();
|
|
}
|
|
else if (shape == physics_shape_HULL) {
|
|
f32_array_t *f32a = (f32_array_t *)f32a_triangles;
|
|
|
|
JPH::Array<JPH::Vec3> points;
|
|
points.reserve(f32a->length / 3);
|
|
for (int i = 0; i < f32a->length / 9; ++i) {
|
|
Vec3 v1(f32a->buffer[i * 9 ], f32a->buffer[i * 9 + 1], f32a->buffer[i * 9 + 2]);
|
|
Vec3 v2(f32a->buffer[i * 9 + 3], f32a->buffer[i * 9 + 4], f32a->buffer[i * 9 + 5]);
|
|
Vec3 v3(f32a->buffer[i * 9 + 6], f32a->buffer[i * 9 + 7], f32a->buffer[i * 9 + 8]);
|
|
points.push_back(v1);
|
|
points.push_back(v2);
|
|
points.push_back(v3);
|
|
}
|
|
|
|
ConvexHullShapeSettings shape_settings(points, convex_radius);
|
|
shape_settings.SetEmbedded();
|
|
result = shape_settings.Create();
|
|
}
|
|
else if (shape == physics_shape_TERRAIN) {
|
|
f32_array_t *f32a = (f32_array_t *)f32a_triangles;
|
|
|
|
// HeightFieldShapeSettings shape_settings();
|
|
// shape_settings.SetEmbedded();
|
|
// result = shape_settings.Create();
|
|
}
|
|
else {
|
|
// Mesh
|
|
f32_array_t *f32a = (f32_array_t *)f32a_triangles;
|
|
TriangleList triangles;
|
|
triangles.reserve(f32a->length / 9);
|
|
for (int i = 0; i < f32a->length / 9; ++i) {
|
|
Float3 v1(f32a->buffer[i * 9 ], f32a->buffer[i * 9 + 1], f32a->buffer[i * 9 + 2]);
|
|
Float3 v2(f32a->buffer[i * 9 + 3], f32a->buffer[i * 9 + 4], f32a->buffer[i * 9 + 5]);
|
|
Float3 v3(f32a->buffer[i * 9 + 6], f32a->buffer[i * 9 + 7], f32a->buffer[i * 9 + 8]);
|
|
triangles.push_back(Triangle(v1, v2, v3));
|
|
}
|
|
|
|
MeshShapeSettings shape_settings(triangles);
|
|
shape_settings.SetEmbedded();
|
|
result = shape_settings.Create();
|
|
}
|
|
|
|
ShapeRefC shape_c = result.Get();
|
|
BodyCreationSettings settings(shape_c, RVec3(x, y, z), Quat::sIdentity(), mass == 0 ? EMotionType::Static : EMotionType::Dynamic,
|
|
mass == 0 ? Layers::NON_MOVING : Layers::MOVING);
|
|
|
|
#ifdef is_forge
|
|
settings.mAllowSleeping = false;
|
|
#endif
|
|
|
|
// if (ccd) {
|
|
if (shape != physics_shape_MESH) {
|
|
settings.mMotionQuality = EMotionQuality::LinearCast;
|
|
|
|
MassProperties mass_prop;
|
|
mass_prop.ScaleToMass(mass);
|
|
settings.mMassPropertiesOverride = mass_prop;
|
|
|
|
settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
|
|
}
|
|
|
|
body = body_interface.CreateBody(settings);
|
|
body_interface.AddBody(body->GetID(), EActivation::Activate);
|
|
|
|
return body;
|
|
}
|
|
|
|
void _jolt_body_apply_impulse(void *b, float x, float y, float z) {
|
|
BodyInterface &body_interface = physics_system->GetBodyInterface();
|
|
Body *body = (Body *)b;
|
|
body_interface.AddImpulse(body->GetID(), Vec3(x, y, z));
|
|
}
|
|
|
|
void _jolt_body_get_pos(void *b, void *p) {
|
|
BodyInterface &body_interface = physics_system->GetBodyInterface();
|
|
Body *body = (Body *)b;
|
|
RVec3 position = body_interface.GetCenterOfMassPosition(body->GetID());
|
|
kinc_vector4_t *v = (kinc_vector4_t *)p;
|
|
v->x = position.GetX();
|
|
v->y = position.GetY();
|
|
v->z = position.GetZ();
|
|
}
|
|
|
|
void _jolt_body_get_rot(void *b, void *r) {
|
|
BodyInterface &body_interface = physics_system->GetBodyInterface();
|
|
Body *body = (Body *)b;
|
|
Quat rotation = body_interface.GetRotation(body->GetID());
|
|
kinc_quaternion_t *q = (kinc_quaternion_t *)r;
|
|
q->x = rotation.GetX();
|
|
q->y = rotation.GetY();
|
|
q->z = rotation.GetZ();
|
|
q->w = rotation.GetW();
|
|
}
|
|
|
|
void _jolt_body_sync_transform(void *b, vec4_t p, quat_t r) {
|
|
BodyInterface &body_interface = physics_system->GetBodyInterface();
|
|
Body *body = (Body *)b;
|
|
body_interface.SetPositionAndRotation(body->GetID(), RVec3(p.x, p.y, p.z), Quat(r.x, r.y, r.z, r.w), EActivation::Activate);
|
|
body_interface.SetLinearVelocity(body->GetID(), RVec3(0, 0, 0));
|
|
body_interface.SetAngularVelocity(body->GetID(), RVec3(0, 0, 0));
|
|
}
|
|
|
|
void _jolt_body_remove(void *b) {
|
|
BodyInterface &body_interface = physics_system->GetBodyInterface();
|
|
Body *body = (Body *)b;
|
|
body_interface.RemoveBody(body->GetID());
|
|
}
|
|
|
|
extern "C" {
|
|
void jolt_world_create() {
|
|
_jolt_world_create();
|
|
}
|
|
|
|
void jolt_world_update() {
|
|
_jolt_world_update();
|
|
}
|
|
|
|
physics_pair_t *jolt_world_get_contact_pairs() {
|
|
return &ppair;
|
|
}
|
|
|
|
void jolt_world_destroy() {
|
|
_jolt_world_destroy();
|
|
}
|
|
|
|
void *jolt_body_create(int shape, float mass, float dimx, float dimy, float dimz, float x, float y, float z, void *f32a_triangles) {
|
|
return _jolt_body_create(shape, mass, dimx, dimy, dimz, x, y, z, f32a_triangles);
|
|
}
|
|
|
|
void jolt_body_apply_impulse(void *body, float x, float y, float z) {
|
|
_jolt_body_apply_impulse(body, x, y, z);
|
|
}
|
|
|
|
void jolt_body_get_pos(void *b, void *p) {
|
|
_jolt_body_get_pos(b, p);
|
|
}
|
|
|
|
void jolt_body_get_rot(void *b, void *r) {
|
|
_jolt_body_get_rot(b, r);
|
|
}
|
|
|
|
void jolt_body_sync_transform(void *b, vec4_t p, quat_t r) {
|
|
_jolt_body_sync_transform(b, p, r);
|
|
}
|
|
|
|
void jolt_body_remove(void *b) {
|
|
_jolt_body_remove(b);
|
|
}
|
|
}
|