singe/src/physicsJolt.cpp

/*
 *
 *  Singe 3
 *  Copyright (C) 2006-2026 Scott Duensing <scott@kangaroopunch.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 3
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301, USA.
 *
 */

// Jolt Physics behind the C interface in physics.h:  the only C++ in Singe.  Nothing of Jolt's
// crosses the header; the engine sees plain C functions and node handles.

#include <SDL3/SDL.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/Body/BodyCreationSettings.h>
#include <Jolt/Physics/Body/BodyLock.h>
#include <Jolt/Physics/Body/BodyLockMulti.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
#include <Jolt/Physics/Collision/ObjectLayer.h>
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
#include <Jolt/Physics/Collision/Shape/MeshShape.h>
#include <Jolt/Physics/Constraints/HingeConstraint.h>
#include <Jolt/Physics/Constraints/PointConstraint.h>
#include <Jolt/Physics/Constraints/SliderConstraint.h>
#include <Jolt/Physics/Collision/CastResult.h>
#include <Jolt/Physics/Collision/RayCast.h>
#include <Jolt/Physics/Collision/ContactListener.h>
#include <mutex>
#include <vector>
extern "C" {
#include "util.h"
#include "scene.h"
}
#include "physics.h"


#define MAX_BODIES            4096
#define MAX_BODY_PAIRS        4096
#define MAX_CONTACTS          8192
#define TEMP_ALLOCATOR_BYTES  (16 * 1024 * 1024)
#define MIN_JOB_THREADS       1
#define STEP_SECONDS          (1.0 / 60.0)
#define MAX_STEPS_PER_FRAME   4
#define MIN_DIMENSION         0.001f
#define DEFAULT_FRICTION      0.5f
#define DEFAULT_BOUNCE        0.1f
#define NO_HANDLE             -1
#define MAX_EVENTS            512           // Per frame; the rest of a busy step is dropped
#define DEFAULT_RAY_DISTANCE  1000.0f
#define WORLD_NODE            -1            // A joint's other side fixed to the world
#define DEGREES_TO_RADIANS(d) ((d) * (3.14159265358979323846f / 180.0f))


// Two object layers:  what never moves and what may.  Static bodies never collide with each other.
namespace {

	const JPH::ObjectLayer     LAYER_NON_MOVING = 0;
	const JPH::ObjectLayer     LAYER_MOVING     = 1;
	const JPH::BroadPhaseLayer BROAD_NON_MOVING(0);
	const JPH::BroadPhaseLayer BROAD_MOVING(1);
	const JPH::uint            BROAD_COUNT      = 2;


	class ObjectPairFilterT final : public JPH::ObjectLayerPairFilter {
	public:
		bool ShouldCollide(JPH::ObjectLayer a, JPH::ObjectLayer b) const override {
			return (a == LAYER_MOVING) || (b == LAYER_MOVING);
		}
	};


	class BroadPhaseLayersT final : public JPH::BroadPhaseLayerInterface {
	public:
		JPH::uint GetNumBroadPhaseLayers() const override {
			return BROAD_COUNT;
		}

		JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer layer) const override {
			return (layer == LAYER_NON_MOVING) ? BROAD_NON_MOVING : BROAD_MOVING;
		}

#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
		const char *GetBroadPhaseLayerName(JPH::BroadPhaseLayer layer) const override {
			return (layer == BROAD_NON_MOVING) ? "NON_MOVING" : "MOVING";
		}
#endif
	};


	class ObjectVsBroadPhaseFilterT final : public JPH::ObjectVsBroadPhaseLayerFilter {
	public:
		bool ShouldCollide(JPH::ObjectLayer layer, JPH::BroadPhaseLayer broad) const override {
			return (layer == LAYER_MOVING) || (broad == BROAD_MOVING);
		}
	};


	// A body on a scene node.
	struct BodyRecordT {
		int32_t     node;
		uint32_t    generation;     // The node's, so a reused handle is not mistaken for this body
		JPH::BodyID id;
		BodyTypeE   type;
		bool        trigger;        // A sensor:  reports overlaps, pushes nothing
		bool        enabled;        // In the world (bodySetEnabled)
		bool        used;
	};


	struct WorldT;
	extern WorldT *_world;
	bool _isTrigger(JPH::BodyID id);


	// Collects contacts from Jolt's job threads; the engine drains it after the step.
	class ContactListenerT final : public JPH::ContactListener {
	public:
		std::mutex                 lock;
		std::vector<PhysicsEventT> events;

		void OnContactAdded(const JPH::Body &a, const JPH::Body &b, const JPH::ContactManifold &manifold, JPH::ContactSettings &settings) override {
			PhysicsEventT event;
			JPH::Vec3     relative;

			(void)settings;
			event.nodeA = (int32_t)(uint32_t)a.GetUserData();
			event.nodeB = (int32_t)(uint32_t)b.GetUserData();
			event.point = vec3((float)manifold.GetWorldSpaceContactPointOn1(0).GetX(), (float)manifold.GetWorldSpaceContactPointOn1(0).GetY(), (float)manifold.GetWorldSpaceContactPointOn1(0).GetZ());
			relative    = a.GetLinearVelocity() - b.GetLinearVelocity();
			event.speed = fabsf(relative.Dot(manifold.mWorldSpaceNormal));
			if (a.IsSensor() || b.IsSensor()) {
				event.type = PHYSICS_EVENT_ENTER;
				if (b.IsSensor()) {
					// The trigger comes first.
					int32_t swap = event.nodeA;

					event.nodeA = event.nodeB;
					event.nodeB = swap;
				}
			} else {
				event.type = PHYSICS_EVENT_COLLISION;
			}
			push(event);
		}

		void OnContactRemoved(const JPH::SubShapeIDPair &pair) override;

		void push(const PhysicsEventT &event) {
			std::lock_guard<std::mutex> guard(lock);

			if (events.size() < MAX_EVENTS) {
				events.push_back(event);
			}
		}
	};


	// A constraint between two bodies (or one body and the world).
	struct JointRecordT {
		JPH::Ref<JPH::Constraint> constraint;
		JointTypeE                type;
		int32_t                   nodeA;
		int32_t                   nodeB;
		bool                      used;
	};


	struct WorldT {
		JPH::TempAllocatorImpl    *tempAllocator;
		JPH::JobSystemThreadPool  *jobs;
		BroadPhaseLayersT          broadPhaseLayers;
		ObjectVsBroadPhaseFilterT  objectVsBroadPhase;
		ObjectPairFilterT          objectPairs;
		JPH::PhysicsSystem        *system;
		ContactListenerT          *contacts;
		BodyRecordT               *bodies;
		int32_t                    bodyCount;
		JointRecordT              *joints;
		int32_t                    jointCount;
		double                     accumulator;    // Seconds owed to the fixed step
		uint64_t                   lastTick;
		bool                       enabled;
		bool                       planar;         // New bodies keep to the XY plane (2D games)
	};


	WorldT *_world = nullptr;


	// A contact ending only matters for triggers:  the pair is reported as left.  This runs inside
	// Jolt's step on a job thread, where taking a body lock deadlocks, so the lock-free interface
	// reads the user data; the trigger flag comes from our records.
	void ContactListenerT::OnContactRemoved(const JPH::SubShapeIDPair &pair) {
		PhysicsEventT event;
		JPH::uint64   userA   = _world->system->GetBodyInterfaceNoLock().GetUserData(pair.GetBody1ID());
		JPH::uint64   userB   = _world->system->GetBodyInterfaceNoLock().GetUserData(pair.GetBody2ID());
		bool          sensorA = _isTrigger(pair.GetBody1ID());
		bool          sensorB = _isTrigger(pair.GetBody2ID());

		if (!sensorA && !sensorB) {
			return;
		}
		event.type  = PHYSICS_EVENT_LEAVE;
		event.nodeA = (int32_t)(uint32_t)(sensorA ? userA : userB);
		event.nodeB = (int32_t)(uint32_t)(sensorA ? userB : userA);
		event.point = vec3(0.0f, 0.0f, 0.0f);
		event.speed = 0.0f;
		push(event);
	}


	JPH::RefConst<JPH::Shape> _buildMeshShape(int32_t node, ShapeTypeE shape, Vec3T position, QuatT rotation);
	void         _collectGeometry(int32_t node, const Mat4T *toBody, JPH::Array<JPH::Vec3> &points, JPH::IndexedTriangleList &triangles, JPH::VertexList &vertices);
	BodyRecordT *_find(int32_t node);
	JPH::Quat    _fromQuat(QuatT q);
	JPH::Vec3    _fromVec3(Vec3T v);
	bool         _isTrigger(JPH::BodyID id);
	void         _release(BodyRecordT *record);
	QuatT        _toQuat(JPH::Quat q);
	Vec3T        _toVec3(JPH::Vec3 v);
	void         _trace(const char *fmt, ...);


	// A convex hull or a triangle mesh from the geometry under a node (its own mesh and every
	// descendant's, a model instance included), in the body's frame:  the node's world scale is
	// baked in, its position and rotation are the body's.
	JPH::RefConst<JPH::Shape> _buildMeshShape(int32_t node, ShapeTypeE shape, Vec3T position, QuatT rotation) {
		JPH::Array<JPH::Vec3>    points;
		JPH::VertexList          vertices;
		JPH::IndexedTriangleList triangles;
		Mat4T                    bodyWorld = mat4Compose(position, rotation, vec3(1.0f, 1.0f, 1.0f));
		Mat4T                    toBody;

		if (!mat4Invert(bodyWorld, &toBody)) {
			return nullptr;
		}
		_collectGeometry(node, &toBody, points, triangles, vertices);
		if (shape == SHAPE_HULL) {
			JPH::ConvexHullShapeSettings settings(points);
			JPH::Shape::ShapeResult      result;

			if (points.size() < 4) {
				utilTrace("Physics:  node %d has too little geometry for a hull.", node);
				return nullptr;
			}
			result = settings.Create();
			if (result.HasError()) {
				utilTrace("Physics:  hull:  %s", result.GetError().c_str());
				return nullptr;
			}
			return result.Get();
		}
		{
			JPH::MeshShapeSettings  settings(vertices, triangles);
			JPH::Shape::ShapeResult result;

			if (triangles.empty()) {
				utilTrace("Physics:  node %d has no triangles for a mesh shape.", node);
				return nullptr;
			}
			result = settings.Create();
			if (result.HasError()) {
				utilTrace("Physics:  mesh:  %s", result.GetError().c_str());
				return nullptr;
			}
			return result.Get();
		}
	}


	// Gathers the node's mesh and its descendants' into the body's frame.
	void _collectGeometry(int32_t node, const Mat4T *toBody, JPH::Array<JPH::Vec3> &points, JPH::IndexedTriangleList &triangles, JPH::VertexList &vertices) {
		const float    *positions;
		const uint32_t *indices;
		int32_t         vertexCount;
		int32_t         indexCount;
		int32_t         mesh = nodeGetMesh(node);
		int32_t         x;

		if ((mesh != NO_HANDLE) && meshGetGeometry(mesh, &positions, &vertexCount, &indices, &indexCount)) {
			Vec3T    position;
			QuatT    rotation;
			Vec3T    scale;
			Mat4T    local;
			uint32_t base = (uint32_t)vertices.size();

			nodeGetWorldTransform(node, &position, &rotation, &scale);
			local = mat4Multiply(*toBody, mat4Compose(position, rotation, scale));
			for (x = 0; x < vertexCount; x++) {
				Vec3T v = mat4TransformPoint(local, vec3(positions[x * 3], positions[x * 3 + 1], positions[x * 3 + 2]));

				points.push_back(JPH::Vec3(v.x, v.y, v.z));
				vertices.push_back(JPH::Float3(v.x, v.y, v.z));
			}
			for (x = 0; x + 2 < indexCount; x += 3) {
				triangles.push_back(JPH::IndexedTriangle(base + indices[x], base + indices[x + 1], base + indices[x + 2]));
			}
		}
		for (x = 0; x < nodeGetChildCount(node); x++) {
			_collectGeometry(nodeGetChild(node, x), toBody, points, triangles, vertices);
		}
	}


	// The record for a node's body, or NULL.  A record whose node was deleted (or reused) is
	// released on the way.
	BodyRecordT *_find(int32_t node) {
		int32_t x;

		if (_world == nullptr) {
			return nullptr;
		}
		for (x = 0; x < _world->bodyCount; x++) {
			BodyRecordT *record = &_world->bodies[x];

			if (!record->used || (record->node != node)) {
				continue;
			}
			if (!nodeValid(node) || (nodeGetGeneration(node) != record->generation)) {
				_release(record);
				return nullptr;
			}
			return record;
		}
		return nullptr;
	}


	JPH::Quat _fromQuat(QuatT q) {
		return JPH::Quat(q.x, q.y, q.z, q.w).Normalized();
	}


	JPH::Vec3 _fromVec3(Vec3T v) {
		return JPH::Vec3(v.x, v.y, v.z);
	}


	// Whether a Jolt body is one of our triggers (by record, so a body being destroyed is safe).
	bool _isTrigger(JPH::BodyID id) {
		int32_t x;

		for (x = 0; x < _world->bodyCount; x++) {
			if (_world->bodies[x].used && (_world->bodies[x].id == id)) {
				return _world->bodies[x].trigger;
			}
		}
		return false;
	}


	// Takes the body out of the world (its joints first) and frees its slot.
	void _release(BodyRecordT *record) {
		JPH::BodyInterface &bodies = _world->system->GetBodyInterface();
		int32_t             x;

		for (x = 0; x < _world->jointCount; x++) {
			if (_world->joints[x].used && ((_world->joints[x].nodeA == record->node) || (_world->joints[x].nodeB == record->node))) {
				jointDelete(x);
			}
		}
		if (record->enabled) {
			bodies.RemoveBody(record->id);
		}
		bodies.DestroyBody(record->id);
		memset(record, 0, sizeof(*record));
	}


	QuatT _toQuat(JPH::Quat q) {
		QuatT out = { q.GetX(), q.GetY(), q.GetZ(), q.GetW() };

		return out;
	}


	Vec3T _toVec3(JPH::Vec3 v) {
		return vec3(v.GetX(), v.GetY(), v.GetZ());
	}


	// Jolt's trace goes to Singe's.
	void _trace(const char *fmt, ...) {
		char    buffer[1024];
		va_list args;

		va_start(args, fmt);
		vsnprintf(buffer, sizeof(buffer), fmt, args);
		va_end(args);
		utilTrace("Physics:  %s", buffer);
	}

}


// ===== Bodies =====

// A force for this step, at the centre of mass or a world point.
bool bodyApplyForce(int32_t node, Vec3T force, const Vec3T *at) {
	BodyRecordT *record = _find(node);

	if ((record == nullptr) || (record->type != BODY_DYNAMIC)) {
		return false;
	}
	if (at != nullptr) {
		_world->system->GetBodyInterface().AddForce(record->id, _fromVec3(force), JPH::RVec3(at->x, at->y, at->z));
	} else {
		_world->system->GetBodyInterface().AddForce(record->id, _fromVec3(force));
	}
	return true;
}


// An instant change of momentum, at the centre of mass or a world point.
bool bodyApplyImpulse(int32_t node, Vec3T impulse, const Vec3T *at) {
	BodyRecordT *record = _find(node);

	if ((record == nullptr) || (record->type != BODY_DYNAMIC)) {
		return false;
	}
	if (at != nullptr) {
		_world->system->GetBodyInterface().AddImpulse(record->id, _fromVec3(impulse), JPH::RVec3(at->x, at->y, at->z));
	} else {
		_world->system->GetBodyInterface().AddImpulse(record->id, _fromVec3(impulse));
	}
	return true;
}


bool bodyDelete(int32_t node) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return false;
	}
	_release(record);
	return true;
}


bool bodyExists(int32_t node) {
	return _find(node) != nullptr;
}


Vec3T bodyGetAngularVelocity(int32_t node) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return vec3(0.0f, 0.0f, 0.0f);
	}
	return _toVec3(_world->system->GetBodyInterface().GetAngularVelocity(record->id));
}


Vec3T bodyGetVelocity(int32_t node) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return vec3(0.0f, 0.0f, 0.0f);
	}
	return _toVec3(_world->system->GetBodyInterface().GetLinearVelocity(record->id));
}


// Asleep:  a dynamic body that has come to rest (static and disabled bodies count as resting).
bool bodyIsResting(int32_t node) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return false;
	}
	return !record->enabled || !_world->system->GetBodyInterface().IsActive(record->id);
}


// Gives the node a body with a shape sized by a, b, c (see ShapeTypeE) and the node's world scale,
// placed where the node is now.  One body per node; a second call replaces the first.
bool bodyNew(int32_t node, BodyTypeE type, ShapeTypeE shape, float a, float b, float c) {
	BodyRecordT               *record;
	JPH::RefConst<JPH::Shape>  joltShape;
	Vec3T                      position;
	QuatT                      rotation;
	Vec3T                      scale;
	float                      radius;
	float                      height;
	int32_t                    x;

	if ((_world == nullptr) || !nodeValid(node)) {
		return false;
	}
	bodyDelete(node);
	sceneUpdateTransforms();
	nodeGetWorldTransform(node, &position, &rotation, &scale);
	switch (shape) {
		case SHAPE_BOX:
			joltShape = new JPH::BoxShape(JPH::Vec3(SDL_max(a * scale.x, MIN_DIMENSION) / 2.0f, SDL_max(b * scale.y, MIN_DIMENSION) / 2.0f, SDL_max(c * scale.z, MIN_DIMENSION) / 2.0f), 0.0f);
			break;
		case SHAPE_SPHERE:
			joltShape = new JPH::SphereShape(SDL_max(a * SDL_max(scale.x, SDL_max(scale.y, scale.z)), MIN_DIMENSION));
			break;
		case SHAPE_CAPSULE:
			radius    = SDL_max(a * SDL_max(scale.x, scale.z), MIN_DIMENSION);
			height    = SDL_max(b * scale.y, MIN_DIMENSION);
			joltShape = new JPH::CapsuleShape(SDL_max(height / 2.0f - radius, MIN_DIMENSION), radius);
			break;
		case SHAPE_CYLINDER:
			radius    = SDL_max(a * SDL_max(scale.x, scale.z), MIN_DIMENSION);
			height    = SDL_max(b * scale.y, MIN_DIMENSION);
			joltShape = new JPH::CylinderShape(height / 2.0f, radius);
			break;
		case SHAPE_HULL:
		case SHAPE_MESH:
			if ((shape == SHAPE_MESH) && (type == BODY_DYNAMIC)) {
				utilTrace("Physics:  a mesh shape can only be static or kinematic; use a hull for node %d.", node);
				return false;
			}
			joltShape = _buildMeshShape(node, shape, position, rotation);
			if (joltShape == nullptr) {
				return false;
			}
			break;
		default:
			utilTrace("Physics:  unknown shape %d.", (int32_t)shape);
			return false;
	}
	for (x = 0; x < _world->bodyCount; x++) {
		if (!_world->bodies[x].used) {
			break;
		}
	}
	if (x == _world->bodyCount) {
		_world->bodies = (BodyRecordT *)SDL_realloc(_world->bodies, sizeof(BodyRecordT) * (size_t)(_world->bodyCount + 1));
		if (_world->bodies == nullptr) {
			utilDie("Out of memory allocating a physics body.");
		}
		_world->bodyCount++;
	}
	record = &_world->bodies[x];
	memset(record, 0, sizeof(*record));
	{
		JPH::EMotionType motion = (type == BODY_STATIC) ? JPH::EMotionType::Static : ((type == BODY_KINEMATIC) ? JPH::EMotionType::Kinematic : JPH::EMotionType::Dynamic);
		JPH::ObjectLayer layer  = (type == BODY_STATIC) ? LAYER_NON_MOVING : LAYER_MOVING;
		JPH::BodyCreationSettings settings(joltShape, JPH::RVec3(position.x, position.y, position.z), _fromQuat(rotation), motion, layer);

		settings.mFriction    = DEFAULT_FRICTION;
		settings.mRestitution = DEFAULT_BOUNCE;
		settings.mUserData    = (JPH::uint64)(uint32_t)node;
		if (_world->planar) {
			// Moves in X and Y, turns about Z, nothing else:  a 2D game's world.
			settings.mAllowedDOFs = JPH::EAllowedDOFs::Plane2D;
		}
		record->id = _world->system->GetBodyInterface().CreateAndAddBody(settings, JPH::EActivation::Activate);
	}
	if (record->id.IsInvalid()) {
		utilTrace("Physics:  unable to create a body (too many?).");
		return false;
	}
	record->node       = node;
	record->generation = nodeGetGeneration(node);
	record->type       = type;
	record->enabled    = true;
	record->used       = true;
	return true;
}


bool bodySetAngularVelocity(int32_t node, Vec3T velocity) {
	BodyRecordT *record = _find(node);

	if ((record == nullptr) || (record->type == BODY_STATIC)) {
		return false;
	}
	_world->system->GetBodyInterface().SetAngularVelocity(record->id, _fromVec3(velocity));
	return true;
}


// 0 stops dead, 1 bounces back with everything it arrived with.
bool bodySetBounce(int32_t node, float bounce) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return false;
	}
	_world->system->GetBodyInterface().SetRestitution(record->id, SDL_clamp(bounce, 0.0f, 1.0f));
	return true;
}


// Takes the body out of the world (it stops colliding and moving) and puts it back.
bool bodySetEnabled(int32_t node, bool enabled) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return false;
	}
	if (enabled && !record->enabled) {
		_world->system->GetBodyInterface().AddBody(record->id, JPH::EActivation::Activate);
	} else if (!enabled && record->enabled) {
		_world->system->GetBodyInterface().RemoveBody(record->id);
	}
	record->enabled = enabled;
	return true;
}


bool bodySetFriction(int32_t node, float friction) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return false;
	}
	_world->system->GetBodyInterface().SetFriction(record->id, SDL_max(friction, 0.0f));
	return true;
}


// Rescales the body's mass and inertia (dynamic bodies only).
bool bodySetMass(int32_t node, float kilograms) {
	BodyRecordT *record = _find(node);

	if ((record == nullptr) || (record->type != BODY_DYNAMIC) || (kilograms <= 0.0f)) {
		return false;
	}
	{
		JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->id);

		if (!lock.Succeeded()) {
			return false;
		}
		lock.GetBody().GetMotionProperties()->ScaleToMass(kilograms);
	}
	return true;
}


// A trigger (Jolt sensor) reports what enters and leaves it and pushes nothing.
bool bodySetTrigger(int32_t node, bool trigger) {
	BodyRecordT *record = _find(node);

	if (record == nullptr) {
		return false;
	}
	{
		JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->id);

		if (!lock.Succeeded()) {
			return false;
		}
		lock.GetBody().SetIsSensor(trigger);
	}
	record->trigger = trigger;
	return true;
}


bool bodySetVelocity(int32_t node, Vec3T velocity) {
	BodyRecordT *record = _find(node);

	if ((record == nullptr) || (record->type == BODY_STATIC)) {
		return false;
	}
	_world->system->GetBodyInterface().SetLinearVelocity(record->id, _fromVec3(velocity));
	return true;
}


// ===== Joints =====

bool jointDelete(int32_t joint) {
	if (!jointValid(joint)) {
		return false;
	}
	_world->system->RemoveConstraint(_world->joints[joint].constraint);
	_world->joints[joint].constraint = nullptr;
	_world->joints[joint].used       = false;
	return true;
}


// A hinge (anchor and axis), ball (anchor) or slider (axis) between two bodies, or between a body
// and the world when nodeB is -1.  Anchor and axis are in world space.  nodeA is Jolt's first
// body (creating them the other way round mirrors the motion); see jointSetLimits for the sign
// that implies.
int32_t jointNew(JointTypeE type, int32_t nodeA, int32_t nodeB, Vec3T anchor, Vec3T axis) {
	BodyRecordT               *a = _find(nodeA);
	BodyRecordT               *b = (nodeB == WORLD_NODE) ? nullptr : _find(nodeB);
	JPH::Ref<JPH::Constraint>  constraint;
	JPH::Vec3                  direction;
	JPH::Vec3                  normal;
	int32_t                    x;

	if ((_world == nullptr) || (a == nullptr) || ((nodeB != WORLD_NODE) && (b == nullptr))) {
		return NO_HANDLE;
	}
	direction = _fromVec3(axis).NormalizedOr(JPH::Vec3::sAxisY());
	normal    = direction.GetNormalizedPerpendicular();
	{
		JPH::BodyID ids[2] = { a->id, (b != nullptr) ? b->id : JPH::BodyID() };
		JPH::BodyLockMultiWrite lock(_world->system->GetBodyLockInterface(), ids, (b != nullptr) ? 2 : 1);
		JPH::Body *bodyA = lock.GetBody(0);
		JPH::Body *bodyB = (b != nullptr) ? lock.GetBody(1) : &JPH::Body::sFixedToWorld;

		if ((bodyA == nullptr) || (bodyB == nullptr)) {
			return NO_HANDLE;
		}
		if (type == JOINT_HINGE) {
			JPH::HingeConstraintSettings settings;

			settings.mSpace       = JPH::EConstraintSpace::WorldSpace;
			settings.mPoint1      = JPH::RVec3(anchor.x, anchor.y, anchor.z);
			settings.mPoint2      = settings.mPoint1;
			settings.mHingeAxis1  = direction;
			settings.mHingeAxis2  = direction;
			settings.mNormalAxis1 = normal;
			settings.mNormalAxis2 = normal;
			constraint = settings.Create(*bodyA, *bodyB);
		} else if (type == JOINT_BALL) {
			JPH::PointConstraintSettings settings;

			settings.mSpace  = JPH::EConstraintSpace::WorldSpace;
			settings.mPoint1 = JPH::RVec3(anchor.x, anchor.y, anchor.z);
			settings.mPoint2 = settings.mPoint1;
			constraint = settings.Create(*bodyA, *bodyB);
		} else {
			JPH::SliderConstraintSettings settings;

			settings.mSpace       = JPH::EConstraintSpace::WorldSpace;
			settings.mPoint1      = JPH::RVec3(anchor.x, anchor.y, anchor.z);
			settings.mPoint2      = settings.mPoint1;
			settings.mSliderAxis1 = direction;
			settings.mSliderAxis2 = direction;
			settings.mNormalAxis1 = normal;
			settings.mNormalAxis2 = normal;
			constraint = settings.Create(*bodyA, *bodyB);
		}
	}
	if (constraint == nullptr) {
		return NO_HANDLE;
	}
	_world->system->AddConstraint(constraint);
	for (x = 0; x < _world->jointCount; x++) {
		if (!_world->joints[x].used) {
			break;
		}
	}
	if (x == _world->jointCount) {
		JointRecordT *grown = new JointRecordT[_world->jointCount + 1];

		for (int32_t y = 0; y < _world->jointCount; y++) {
			grown[y] = _world->joints[y];
		}
		delete[] _world->joints;
		_world->joints = grown;
		_world->jointCount++;
	}
	_world->joints[x].constraint = constraint;
	_world->joints[x].type       = type;
	_world->joints[x].nodeA      = nodeA;
	_world->joints[x].nodeB      = nodeB;
	_world->joints[x].used       = true;
	return x;
}


// Limits:  degrees either side of the starting angle for a hinge (positive is a right-hand turn of
// nodeA about the axis), distance along the axis for a slider (positive along it); low at or below
// 0, high at or above.  Jolt measures both as its second body relative to its first, which is the
// opposite of nodeA's own motion, so the range is mirrored on the way in.  A ball joint has none.
bool jointSetLimits(int32_t joint, float low, float high) {
	if (!jointValid(joint)) {
		return false;
	}
	low  = SDL_min(low, 0.0f);
	high = SDL_max(high, 0.0f);
	if (_world->joints[joint].type == JOINT_HINGE) {
		((JPH::HingeConstraint *)_world->joints[joint].constraint.GetPtr())->SetLimits(DEGREES_TO_RADIANS(-high), DEGREES_TO_RADIANS(-low));
		return true;
	}
	if (_world->joints[joint].type == JOINT_SLIDER) {
		((JPH::SliderConstraint *)_world->joints[joint].constraint.GetPtr())->SetLimits(-high, -low);
		return true;
	}
	return false;
}


bool jointValid(int32_t joint) {
	return (_world != nullptr) && (joint >= 0) && (joint < _world->jointCount) && _world->joints[joint].used;
}


// ===== World =====

bool physicsAvailable(void) {
	return _world != nullptr;
}


// Hands the engine the events the last step produced (up to maximum) and clears them.
int32_t physicsGetEvents(PhysicsEventT *events, int32_t maximum) {
	int32_t count = 0;

	if (_world == nullptr) {
		return 0;
	}
	{
		std::lock_guard<std::mutex> guard(_world->contacts->lock);

		while ((count < maximum) && (count < (int32_t)_world->contacts->events.size())) {
			events[count] = _world->contacts->events[(size_t)count];
			count++;
		}
		_world->contacts->events.clear();
	}
	return count;
}


// Brings Jolt up:  allocators, the type factory, a job pool sized to the machine, and an empty
// world.  Refuses (returning false, 3D physics unavailable) on an x86 without SSE4.1 and 4.2, the
// level the library was compiled for, rather than faulting on the first instruction.
bool physicsInit(void) {
	int32_t threads;

	if (_world != nullptr) {
		return true;
	}
#if defined(JPH_USE_SSE4_2)
	if (!SDL_HasSSE41() || !SDL_HasSSE42()) {
		utilTrace("Physics:  this CPU lacks SSE4.1/4.2; physics is unavailable.");
		return false;
	}
#endif
	JPH::RegisterDefaultAllocator();
	JPH::Trace = _trace;
	JPH::Factory::sInstance = new JPH::Factory();
	JPH::RegisterTypes();
	_world = new WorldT();
	_world->tempAllocator = new JPH::TempAllocatorImpl(TEMP_ALLOCATOR_BYTES);
	threads = SDL_max(SDL_GetNumLogicalCPUCores() - 1, MIN_JOB_THREADS);
	_world->jobs   = new JPH::JobSystemThreadPool(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, threads);
	_world->system = new JPH::PhysicsSystem();
	_world->system->Init(MAX_BODIES, 0, MAX_BODY_PAIRS, MAX_CONTACTS, _world->broadPhaseLayers, _world->objectVsBroadPhase, _world->objectPairs);
	_world->contacts = new ContactListenerT();
	_world->system->SetContactListener(_world->contacts);
	_world->enabled = true;
	utilTrace("Physics:  Jolt %d.%d.%d ready, %d job thread%s", JPH_VERSION_MAJOR, JPH_VERSION_MINOR, JPH_VERSION_PATCH, threads, (threads == 1) ? "" : "s");
	return true;
}


void physicsQuit(void) {
	int32_t x;

	if (_world == nullptr) {
		return;
	}
	for (x = 0; x < _world->bodyCount; x++) {
		if (_world->bodies[x].used) {
			_release(&_world->bodies[x]);
		}
	}
	SDL_free(_world->bodies);
	delete[] _world->joints;
	delete _world->system;
	delete _world->contacts;
	delete _world->jobs;
	delete _world->tempAllocator;
	delete _world;
	_world = nullptr;
	JPH::UnregisterTypes();
	delete JPH::Factory::sInstance;
	JPH::Factory::sInstance = nullptr;
}


// The nearest body along a ray (triggers included), with the hit point and surface normal.
bool physicsRaycast(Vec3T origin, Vec3T direction, float maxDistance, int32_t *node, Vec3T *point, Vec3T *normal) {
	JPH::RayCastResult result;
	JPH::Vec3          dir;
	JPH::RVec3         hit;

	if (_world == nullptr) {
		return false;
	}
	dir = _fromVec3(direction).NormalizedOr(JPH::Vec3::sZero());
	if (dir.IsNearZero()) {
		return false;
	}
	if (maxDistance <= 0.0f) {
		maxDistance = DEFAULT_RAY_DISTANCE;
	}
	{
		JPH::RRayCast ray(JPH::RVec3(origin.x, origin.y, origin.z), dir * maxDistance);

		if (!_world->system->GetNarrowPhaseQuery().CastRay(ray, result)) {
			return false;
		}
		hit = ray.GetPointOnRay(result.mFraction);
	}
	*node  = (int32_t)(uint32_t)_world->system->GetBodyInterface().GetUserData(result.mBodyID);
	*point = vec3((float)hit.GetX(), (float)hit.GetY(), (float)hit.GetZ());
	{
		JPH::BodyLockRead lock(_world->system->GetBodyLockInterface(), result.mBodyID);

		*normal = lock.Succeeded() ? _toVec3(lock.GetBody().GetWorldSpaceSurfaceNormal(result.mSubShapeID2, hit)) : vec3(0.0f, 1.0f, 0.0f);
	}
	return true;
}


// 2D mode for bodies made from now on:  they move in X and Y and turn about Z only, so a 2D game
// can run its world in overlay coordinates (gravity pointing +Y then, since the overlay's Y runs
// down) and draw sprites where the nodes are.  Bodies already made keep their freedom.
void physicsSet2D(bool planar) {
	if (_world != nullptr) {
		_world->planar = planar;
	}
}


// Pauses the simulation (bodies hold still) without losing it.
void physicsSetEnabled(bool enabled) {
	if (_world != nullptr) {
		_world->enabled = enabled;
	}
}


void physicsSetGravity(Vec3T gravity) {
	if (_world != nullptr) {
		_world->system->SetGravity(_fromVec3(gravity));
	}
}


// Once per frame, after animation:  kinematic bodies are moved to their nodes, the world steps at
// a fixed rate for the time that has passed (at most a few steps, and none while the game is
// paused or physics is disabled), and dynamic bodies drive their nodes.
void physicsUpdate(bool advance) {
	uint64_t now;
	int32_t  steps = 0;
	int32_t  x;
	Vec3T    position;
	QuatT    rotation;
	Vec3T    scale;

	if (_world == nullptr) {
		return;
	}
	now = SDL_GetTicksNS();
	if (advance && _world->enabled && (_world->lastTick != 0)) {
		_world->accumulator += (double)(now - _world->lastTick) / 1e9;
	}
	_world->lastTick = now;
	if (_world->accumulator > STEP_SECONDS * MAX_STEPS_PER_FRAME) {
		_world->accumulator = STEP_SECONDS * MAX_STEPS_PER_FRAME;
	}
	if (_world->accumulator < STEP_SECONDS) {
		return;
	}
	sceneUpdateTransforms();
	{
		JPH::BodyInterface &bodies = _world->system->GetBodyInterface();
		float               dt     = (float)(STEP_SECONDS * (int32_t)(_world->accumulator / STEP_SECONDS));

		// Kinematic bodies go where their nodes went, with the velocity that implies.
		for (x = 0; x < _world->bodyCount; x++) {
			BodyRecordT *record = &_world->bodies[x];

			if (!record->used || (record->type != BODY_KINEMATIC) || !record->enabled) {
				continue;
			}
			if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
				_release(record);
				continue;
			}
			nodeGetWorldTransform(record->node, &position, &rotation, &scale);
			bodies.MoveKinematic(record->id, JPH::RVec3(position.x, position.y, position.z), _fromQuat(rotation), dt);
		}
		while ((_world->accumulator >= STEP_SECONDS) && (steps < MAX_STEPS_PER_FRAME)) {
			_world->system->Update((float)STEP_SECONDS, 1, _world->tempAllocator, _world->jobs);
			_world->accumulator -= STEP_SECONDS;
			steps++;
		}
		// Dynamic bodies drive their nodes.
		for (x = 0; x < _world->bodyCount; x++) {
			BodyRecordT *record = &_world->bodies[x];
			JPH::RVec3   where;
			JPH::Quat    how;

			if (!record->used || (record->type != BODY_DYNAMIC) || !record->enabled) {
				continue;
			}
			if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
				_release(record);
				continue;
			}
			bodies.GetPositionAndRotation(record->id, where, how);
			nodeSetWorldTransform(record->node, vec3((float)where.GetX(), (float)where.GetY(), (float)where.GetZ()), _toQuat(how));
		}
	}
}