/* * * Singe 3 * Copyright (C) 2006-2026 Scott Duensing * * 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef JPH_DEBUG_RENDERER #include #endif #include #include #include extern "C" { #include "util.h" #include "scene.h" #include "model.h" } #include "physics.h" #define MAX_BODIES 4096 #define MAX_PLAYERS 16 #define MAX_VEHICLES 16 #define MAX_WATERS 8 #define MAX_PLAYER_TRIGGERS 16 #define MAX_RAGDOLLS 8 #define MAX_SOFT 16 #define MAX_SOFT_PINS 32 #define ROPE_SIDES 6 #define ROPE_INDICES_PER_QUAD 6 // Two triangles between neighbouring rings #define SOFT_WELD 1.0e-4f // Mesh vertices this close are one particle #define SOFT_VERTEX_RADIUS 0.03f // Cloth and pressure bodies; a rope uses its own radius #define STRETCH_COMPLIANCE 1.0e-3f // At stiffness 0; stiffness 1 is rigid #define BEND_COMPLIANCE 1.0e-2f #define DEFAULT_SOFT_STRETCH 0.9f #define DEFAULT_SOFT_BEND 0.2f #define DEFAULT_SOFT_MASS 1.0f #define DEFAULT_SOFT_DAMPING 0.1f #define SOFT_ITERATIONS 8 #define MAX_RAGDOLL_PARTS 48 #define RAGDOLL_RADIUS_RATIO 0.22f // Capsule radius as a share of the bone length ... #define RAGDOLL_RADIUS_MIN 0.02f // ... and never thinner than this #define RAGDOLL_SWING_DEGREES 45.0f #define RAGDOLL_TWIST_DEGREES 30.0f #define RAGDOLL_MOTOR_HZ 4.0f #define RAGDOLL_MOTOR_DAMPING 1.0f #define RAGDOLL_LINEAR_DAMPING 0.2f #define RAGDOLL_ANGULAR_DAMPING 0.5f #define LEAF_BONE_RADII 4.0f // A bone with no child joint is this many minimum radii long ... #define MIN_BONE_RADII 2.0f // ... and no bone is shorter than this many #define MAX_CONE_DEGREES 179.0f // Widest swing or twist a ragdoll joint allows #define DEFAULT_BUOYANCY 1.2f #define DEFAULT_SINK_SPEED 0.3f #define DEFAULT_SWIM_DRAG 2.0f #define MAX_WHEELS 16 #define DEFAULT_ENGINE_TORQUE 500.0f #define DEFAULT_ENGINE_MAX_RPM 6000.0f #define MIN_ENGINE_TORQUE 1.0f #define MIN_ENGINE_RPM 1.0f // The lowest idle a script may ask for ... #define MIN_ENGINE_MAX_RPM 100.0f // ... and the lowest redline #define DEFAULT_STEER_DEGREES 35.0f #define MAX_STEER_DEGREES 89.0f #define DEFAULT_BRAKE_TORQUE 1500.0f #define DEBUG_CONTACT_SIZE 0.1f // Half the cross drawn at a contact #define DEFAULT_HANDBRAKE_TORQUE 4000.0f #define DEFAULT_SUSPENSION_HZ 1.5f #define DEFAULT_SUSPENSION_DAMPING 0.5f #define MIN_SUSPENSION_HZ 0.1f #define VEHICLE_MAX_TILT_DEGREES 60.0f #define FRONT_EPSILON 1.0e-4f // A wheel this far ahead of the mean is a front wheel #define WHEEL_CAST_RADIUS 0.05f // Convex radius of the cylinder a wheel feels the ground with #define MIN_INVERSE_INERTIA 1.0e-9f // Below this the chassis' roll inertia is taken as LEAN_INERTIA #define TANK_PIVOT_THROTTLE 0.35f // Track drive when a tank turns on the spot #define TANK_TURN_RATIO 0.9f // How much the inner track slows at full steer ... #define TANK_TURN_MIN 0.1f // ... but never below this #define DEFAULT_THRUST 2000.0f // Boats: propeller force ... #define DEFAULT_THRUST_Y -0.2f // ... applied here in the hull's frame ... #define DEFAULT_THRUST_Z 1.0f #define DEFAULT_RUDDER 800.0f // ... and turning torque ... #define RUDDER_FULL_SPEED 3.0f // ... which bites fully from this speed ... #define RUDDER_MIN_BITE 0.2f // ... and this much when still #define BOAT_BRAKE_RATIO 0.5f // A boat's braking force as a share of its thrust #define LEAN_SPRING 5000.0f // Jolt's motorcycle lean spring and damping ... #define LEAN_DAMPING 1000.0f #define LEAN_INERTIA 40.0f // ... for a chassis of this roll inertia (kg m^2); scaled from there #define DEFAULT_STEP_HEIGHT 0.3f // For a DEFAULT_EXTENT player; scales with the shape #define DEFAULT_EXTENT 0.3f // The half-extent Jolt's character defaults were tuned for #define PADDING_PER_EXTENT (0.02f / 0.3f) #define PREDICTIVE_PER_EXTENT (0.1f / 0.3f) #define TOLERANCE_PER_EXTENT (0.001f / 0.3f) #define STICK_PER_EXTENT (0.5f / 0.3f) #define STEP_TEST_PER_EXTENT (0.15f / 0.3f) #define DEFAULT_SLOPE_DEGREES 45.0f #define MAX_SLOPE_DEGREES 89.0f #define DEFAULT_PUSH_STRENGTH 300.0f #define STANDING_VERTICAL_SPEED 0.1f // Rising faster than this off the ground counts as airborne #define ZERO_GRAVITY_SQ 1.0e-8f // Gravity shorter than this leaves up as +Y #define MAX_BODY_PAIRS 65536 // Broad phase pairs a step may find: many more than touch #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 MIN_MASS 0.001f #define MIN_SCALE 1.0e-6f // A node axis scaled below this is not divided by #define ZERO_LENGTH_SQ 1.0e-10f #define DEFAULT_FRICTION 0.5f #define DEFAULT_BOUNCE 0.1f #define NO_HANDLE -1 #define DEFAULT_RAY_DISTANCE 1000.0f #define WORLD_NODE -1 // A joint's other side fixed to the world // 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; bool water; // A trigger full of water (bodySetWater) float waterDensity; float waterLinearDrag; float waterAngularDrag; Vec3T current; // Flow inside the water float buoyancy; // How this body floats in water (bodySetBuoyancy) }; // A water surface found this step, for players and boats to test against. struct WaterSurfaceT { JPH::AABox box; JPH::RVec3 position; JPH::Vec3 normal; JPH::Vec3 current; }; // A character controller on a node: Jolt's CharacterVirtual, moved by intent rather than force. struct PlayerRecordT { int32_t node; uint32_t generation; JPH::Ref character; float extent; // Smallest half-size of the shape: the unit the tolerances scale by float height; // Of the shape, for the swimming test float sinkSpeed; // Swimming: how fast it sinks with no input float swimDrag; bool swimming; JPH::Vec3 swimCurrent; int32_t inside[MAX_PLAYER_TRIGGERS]; // Trigger nodes the player is in int32_t insideCount; Vec3T intent; // Wanted horizontal velocity, consumed by the next steps float jumpSpeed; // Pending jump, consumed by the next step float gravityScale; float stepHeight; bool enabled; bool used; }; // One bone of a ragdoll: a joint node with at least one child joint, as a capsule body. struct RagdollPartT { int32_t joint; int32_t parent; // Part index of the nearest ancestor joint with a part, or -1 float radius; // 0: from the bone length float swing; // Degrees float twist; JPH::BodyID body; JPH::Ref constraint; JPH::Vec3 offsetPosition; // The joint's origin in the body's frame JPH::Quat offsetRotation; // The joint's rotation in the body's frame JPH::Quat rest; // This body's rotation relative to the parent body when activated JPH::Vec3 scale; // The joint's world scale, kept for its children }; // A skinned model's skeleton as bodies and joints, switched on and off. struct RagdollRecordT { int32_t node; // The model instance's root uint32_t generation; int32_t skinned; // The node carrying the skin RagdollPartT parts[MAX_RAGDOLL_PARTS]; int32_t partCount; JPH::Ref filter; float strength; bool strengthChanged; bool active; bool used; }; // A mesh vertex's position quantised to SOFT_WELD cells, for welding vertices into particles. struct WeldKeyT { int32_t x; int32_t y; int32_t z; bool operator==(const WeldKeyT &other) const { return (x == other.x) && (y == other.y) && (z == other.z); } }; struct WeldHashT { size_t operator()(const WeldKeyT &key) const { return ((size_t)(uint32_t)key.x * 73856093u) ^ ((size_t)(uint32_t)key.y * 19349663u) ^ ((size_t)(uint32_t)key.z * 83492791u); } }; // A vertex of a soft body held in place, or held to a node. struct SoftPinT { int32_t vertex; int32_t follow; // -1: held where it was pinned }; // A soft body driving a mesh: cloth or a pressure body from the node's own mesh (its vertices // welded by position into particles), or a rope whose tube mesh the engine makes and moves. struct SoftRecordT { int32_t node; uint32_t generation; SoftKindE kind; int32_t mesh; JPH::BodyID body; JPH::Ref shared; int32_t *meshToSoft; // Mesh vertex to particle float *positions; // Particles, world space int32_t count; float *meshPositions; // Scratch for the mesh rewrite int32_t meshVertexCount; SoftPinT pins[MAX_SOFT_PINS]; int32_t pinCount; float stretch; float bend; float pressure; float mass; float damping; float ropeRadius; bool used; }; // A wheel of a vehicle: the node the engine poses, and its geometry relative to the chassis. struct WheelRecordT { int32_t node; uint32_t generation; Vec3T rest; // Where the wheel sits in the chassis' frame, taken when it was added float radius; float width; float suspension; bool steered; bool driven; bool steeredSet; // vehicleSetWheel called; otherwise front wheels steer and all drive }; // A vehicle on a chassis body: the recipe, and the Jolt constraint built from it on demand. struct VehicleRecordT { int32_t node; uint32_t generation; VehicleKindE kind; JPH::Ref constraint; JPH::Ref tester; WheelRecordT wheels[MAX_WHEELS]; int32_t wheelCount; float maxTorque; float maxRpm; float minRpm; float gears[VEHICLE_MAX_GEARS]; int32_t gearCount; float reverseGear; bool automatic; float suspensionHz; float suspensionDamping; float maxSteer; // Degrees float brakeTorque; float handBrakeTorque; float antiRoll; float inputForward; float inputRight; float inputBrake; float inputHandBrake; float thrust; // Boats: propeller force ... Vec3T thrustPoint; // ... applied here in the hull's frame float rudder; // ... and turning torque bool dirty; // Settings changed: rebuild before the next step bool used; }; // Collects contacts from Jolt's job threads; the engine drains it after the step. Trigger // overlaps are counted per (trigger, body) pair, since Jolt reports every sub-shape pair on its // own (a mesh trigger: one per triangle) and the engine wants one enter and one leave. class ContactListenerT final : public JPH::ContactListener { public: std::mutex lock; std::vector events; std::unordered_map overlaps; // (trigger, body) to sub-shape pairs touching void OnContactAdded(const JPH::Body &a, const JPH::Body &b, const JPH::ContactManifold &manifold, JPH::ContactSettings &settings) override; void OnContactRemoved(const JPH::SubShapeIDPair &pair) override; void forget(int32_t node, bool everyRole); // Drops the overlap counts where node is the trigger (or, everyRole, either party) void overlap(int32_t trigger, int32_t node, bool entered); // One sub-shape pair started or stopped touching void push(const PhysicsEventT &event); // Queues an event, taking the lock void pushLocked(const PhysicsEventT &event); // Queues an event under a lock already held }; // A constraint between two bodies (or one body and the world). struct JointRecordT { JPH::Ref constraint; JointTypeE type; int32_t nodeA; int32_t nodeB; bool used; }; // Contacts between a player and bodies, reported like body contacts: the player's node first, // or the trigger first when the other is a sensor. These arrive from the character's own update // on the engine thread, but share the body listener's queue and lock. class PlayerListenerT final : public JPH::CharacterContactListener { public: bool OnContactValidate(const JPH::CharacterVirtual *character, const JPH::CharacterContact &contact) override; void OnContactAdded(const JPH::CharacterVirtual *character, const JPH::CharacterContact &contact, JPH::CharacterContactSettings &settings) override; }; // The tables are fixed-size (bodyCount is the highest slot ever used, so loops stay short); // each is indexed by node handle through a vector grown on demand, since handles are small // dense integers. struct WorldT { JPH::TempAllocatorImpl *tempAllocator; JPH::JobSystemThreadPool *jobs; BroadPhaseLayersT broadPhaseLayers; ObjectVsBroadPhaseFilterT objectVsBroadPhase; ObjectPairFilterT objectPairs; JPH::PhysicsSystem *system; ContactListenerT *contacts; BodyRecordT *bodies; int32_t bodyCount; std::vector joints; PlayerRecordT *players; int32_t playerCount; PlayerListenerT *playerListener; VehicleRecordT *vehicles; int32_t vehicleCount; RagdollRecordT *ragdolls; int32_t ragdollCount; SoftRecordT *softs; int32_t softCount; std::vector bodyOfNode; // Node handle to slot in each table, or NO_HANDLE std::vector playerOfNode; std::vector vehicleOfNode; std::vector ragdollOfNode; std::vector softOfNode; JPH::uint32 ragdollGroups; // Next collision group id WaterSurfaceT waters[MAX_WATERS]; int32_t waterCount; 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; void _applyWater(float dt); JPH::RefConst _buildHeightField(int32_t mesh, Vec3T scale); JPH::RefConst _buildMeshShape(int32_t node, ShapeTypeE shape, Vec3T position, QuatT rotation); JPH::RefConst _buildShape(int32_t node, BodyTypeE type, ShapeTypeE shape, float a, float b, float c, Vec3T position, QuatT rotation, Vec3T scale); bool _buildSoft(SoftRecordT *record); bool _buildVehicle(VehicleRecordT *record); void _collectGeometry(int32_t node, const Mat4T *toBody, ShapeTypeE shape, JPH::Array &points, JPH::IndexedTriangleList &triangles, JPH::VertexList &vertices); void _destroyBody(JPH::BodyID &id); void _drawDebug(void); void _driveVehicles(void); BodyRecordT *_find(int32_t node); PlayerRecordT *_findPlayer(int32_t node); RagdollRecordT *_findRagdoll(int32_t node); template RecordT *_findRecord(std::vector &index, RecordT *table, int32_t node, void (*release)(RecordT *)); int32_t _findSkinned(int32_t node); SoftRecordT *_findSoft(int32_t node); VehicleRecordT *_findVehicle(int32_t node); JPH::Quat _fromQuat(QuatT q); JPH::Vec3 _fromVec3(Vec3T v); int32_t _indexGet(const std::vector &index, int32_t node); void _indexSet(std::vector &index, int32_t node, int32_t slot); bool _isPlayer(int32_t node); int32_t _nearestSoftVertex(const SoftRecordT *record, Vec3T point); bool _nodeStale(int32_t node, uint32_t generation); void _pinSoft(void); void _playerInside(PlayerRecordT *record, const int32_t *now, int32_t count); void _playerTriggers(PlayerRecordT *record); JPH::Vec3 _playerUp(void); void _poseWheels(void); int32_t _ragdollPartOf(RagdollRecordT *record, int32_t joint); void _release(BodyRecordT *record); void _releaseDead(void); void _releasePlayer(PlayerRecordT *record); void _releaseRagdoll(RagdollRecordT *record); void _releaseRagdollBodies(RagdollRecordT *record); void _releaseSoft(SoftRecordT *record); void _releaseSoftBody(SoftRecordT *record); template void _releaseStale(RecordT *table, int32_t count, void (*release)(RecordT *)); void _releaseVehicle(VehicleRecordT *record); void _resetRagdoll(RagdollRecordT *record); void _resetSoft(SoftRecordT *record); void _resetVehicle(VehicleRecordT *record); void _ropeMesh(const SoftRecordT *record, Vec3T *out); void _setDrivetrain(const VehicleRecordT *record, JPH::VehicleEngineSettings &engine, JPH::VehicleTransmissionSettings &transmission); float _softInvMass(const SoftRecordT *record); void _softSetMasses(SoftRecordT *record); void _steerRagdolls(void); void _step(void); void _stepPlayers(float dt); QuatT _toQuat(JPH::Quat q); Vec3T _toVec3(JPH::Vec3 v); void _trace(const char *fmt, ...); PhysicsEventT _triggerEvent(PhysicsEventTypeE type, int32_t trigger, int32_t node); int32_t _triggerNode(JPH::BodyID id); bool _underWater(JPH::RVec3Arg point, JPH::Vec3 *current); void _writePlayers(void); void _writeRagdolls(void); void _writeSoft(void); uint32_t _debugMask = DEBUG_NONE; void ContactListenerT::OnContactAdded(const JPH::Body &a, const JPH::Body &b, const JPH::ContactManifold &manifold, JPH::ContactSettings &settings) { PhysicsEventT event; JPH::Vec3 relative; (void)settings; if (a.IsSensor() || b.IsSensor()) { const JPH::Body &trigger = a.IsSensor() ? a : b; const JPH::Body &other = a.IsSensor() ? b : a; int32_t node = (int32_t)(uint32_t)other.GetUserData(); // A player's inner body: the engine reports players entering triggers itself. if (!_isPlayer(node)) { overlap((int32_t)(uint32_t)trigger.GetUserData(), node, true); } return; } event.type = PHYSICS_EVENT_COLLISION; event.nodeA = (int32_t)(uint32_t)a.GetUserData(); event.nodeB = (int32_t)(uint32_t)b.GetUserData(); event.point = _toVec3(JPH::Vec3(manifold.GetWorldSpaceContactPointOn1(0))); relative = a.GetLinearVelocity() - b.GetLinearVelocity(); event.speed = fabsf(relative.Dot(manifold.mWorldSpaceNormal)); push(event); } // A contact ending only matters for triggers. This runs inside Jolt's step on a job thread, // where the locking interface deadlocks, so the bodies are read through the lock-free one (a // body destroyed since the contact was made simply fails to lock). void ContactListenerT::OnContactRemoved(const JPH::SubShapeIDPair &pair) { JPH::BodyLockRead lockA(_world->system->GetBodyLockInterfaceNoLock(), pair.GetBody1ID()); JPH::BodyLockRead lockB(_world->system->GetBodyLockInterfaceNoLock(), pair.GetBody2ID()); const JPH::Body *trigger; const JPH::Body *other; int32_t node; if (!lockA.Succeeded() || !lockB.Succeeded()) { return; } if (lockA.GetBody().IsSensor()) { trigger = &lockA.GetBody(); other = &lockB.GetBody(); } else if (lockB.GetBody().IsSensor()) { trigger = &lockB.GetBody(); other = &lockA.GetBody(); } else { return; } node = (int32_t)(uint32_t)other->GetUserData(); if (!_isPlayer(node)) { overlap((int32_t)(uint32_t)trigger->GetUserData(), node, false); } } // A body that stops being a trigger keeps the overlaps it is inside as a body; a body that is // gone drops both. void ContactListenerT::forget(int32_t node, bool everyRole) { std::lock_guard guard(lock); std::unordered_map::iterator it = overlaps.begin(); while (it != overlaps.end()) { if (((int32_t)(uint32_t)(it->first >> 32) == node) || (everyRole && ((int32_t)(uint32_t)it->first == node))) { it = overlaps.erase(it); } else { ++it; } } } void ContactListenerT::overlap(int32_t trigger, int32_t node, bool entered) { uint64_t key = ((uint64_t)(uint32_t)trigger << 32) | (uint32_t)node; std::lock_guard guard(lock); if (entered) { if (++overlaps[key] != 1) { return; } } else { std::unordered_map::iterator it = overlaps.find(key); if ((it == overlaps.end()) || (--it->second > 0)) { return; } overlaps.erase(it); } pushLocked(_triggerEvent(entered ? PHYSICS_EVENT_ENTER : PHYSICS_EVENT_LEAVE, trigger, node)); } void ContactListenerT::push(const PhysicsEventT &event) { std::lock_guard guard(lock); pushLocked(event); } void ContactListenerT::pushLocked(const PhysicsEventT &event) { if (events.size() < PHYSICS_MAX_EVENTS) { events.push_back(event); } } #ifdef JPH_DEBUG_RENDERER // Jolt's debug drawing lands in the scene's line list: triangles as their edges, text dropped. class LineRendererT final : public JPH::DebugRendererSimple { public: void DrawLine(JPH::RVec3Arg from, JPH::RVec3Arg to, JPH::ColorArg colour) override { sceneDrawLine(_toVec3(from), _toVec3(to), colour.r, colour.g, colour.b); } void DrawTriangle(JPH::RVec3Arg a, JPH::RVec3Arg b, JPH::RVec3Arg c, JPH::ColorArg colour, ECastShadow shadow) override { (void)shadow; DrawLine(a, b, colour); DrawLine(b, c, colour); DrawLine(c, a, colour); } void DrawText3D(JPH::RVec3Arg position, const JPH::string_view &text, JPH::ColorArg colour, float height) override { (void)position; (void)text; (void)colour; (void)height; } }; // Static bodies only draw when asked: a level mesh is a lot of lines. class MovingFilterT final : public JPH::BodyDrawFilter { public: bool ShouldDraw(const JPH::Body &body) const override { return !body.IsStatic(); } }; LineRendererT *_renderer = nullptr; #endif // What physicsSetDebug asked for, as lines in the scene for this frame. void _drawDebug(void) { #ifdef JPH_DEBUG_RENDERER JPH::BodyManager::DrawSettings settings; MovingFilterT moving; int32_t x; if (_debugMask == DEBUG_NONE) { return; } if (_renderer == nullptr) { _renderer = new LineRendererT(); } settings.mDrawShape = (_debugMask & DEBUG_SHAPES) != 0; settings.mDrawShapeWireframe = true; settings.mDrawSoftBodyEdgeConstraints = settings.mDrawShape; settings.mDrawVelocity = (_debugMask & DEBUG_VELOCITIES) != 0; if (settings.mDrawShape || settings.mDrawVelocity) { _world->system->DrawBodies(settings, _renderer, (_debugMask & DEBUG_STATIC) ? nullptr : &moving); } if (_debugMask & DEBUG_CONSTRAINTS) { _world->system->DrawConstraints(_renderer); _world->system->DrawConstraintLimits(_renderer); } if (_debugMask & DEBUG_SHAPES) { for (x = 0; x < _world->playerCount; x++) { PlayerRecordT *record = &_world->players[x]; if (record->used && record->enabled) { record->character->GetShape()->Draw(_renderer, record->character->GetCenterOfMassTransform(), JPH::Vec3::sOne(), JPH::Color::sYellow, false, true); } } } if (_debugMask & DEBUG_CONTACTS) { std::lock_guard guard(_world->contacts->lock); for (const PhysicsEventT &event : _world->contacts->events) { if (event.type == PHYSICS_EVENT_COLLISION) { JPH::Vec3 at = _fromVec3(event.point); _renderer->DrawLine(at - JPH::Vec3::sAxisX() * DEBUG_CONTACT_SIZE, at + JPH::Vec3::sAxisX() * DEBUG_CONTACT_SIZE, JPH::Color::sRed); _renderer->DrawLine(at - JPH::Vec3::sAxisY() * DEBUG_CONTACT_SIZE, at + JPH::Vec3::sAxisY() * DEBUG_CONTACT_SIZE, JPH::Color::sRed); _renderer->DrawLine(at - JPH::Vec3::sAxisZ() * DEBUG_CONTACT_SIZE, at + JPH::Vec3::sAxisZ() * DEBUG_CONTACT_SIZE, JPH::Color::sRed); } } } #endif } // Runs the fixed steps the accumulator owes: kinematic bodies go where their nodes went first, // dynamic bodies drive their nodes afterwards. void _step(void) { int32_t steps = 0; int32_t x; Vec3T position; QuatT rotation; Vec3T scale; 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; } 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)) { _applyWater((float)STEP_SECONDS); _driveVehicles(); _steerRagdolls(); _pinSoft(); _stepPlayers((float)STEP_SECONDS); _world->system->Update((float)STEP_SECONDS, 1, _world->tempAllocator, _world->jobs); _world->accumulator -= STEP_SECONDS; steps++; } _writePlayers(); _poseWheels(); _writeRagdolls(); _writeSoft(); // 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; } bodies.GetPositionAndRotation(record->id, where, how); nodeSetWorldTransform(record->node, vec3((float)where.GetX(), (float)where.GetY(), (float)where.GetZ()), _toQuat(how)); } } } // A height field from a heightmap mesh's samples, resampled to the square even count Jolt wants, // with the node's scale baked into the sizes; null for any other mesh. JPH::RefConst _buildHeightField(int32_t mesh, Vec3T scale) { const float *heights; int32_t columns; int32_t rows; float sizeX; float sizeY; float sizeZ; int32_t n; int32_t x; int32_t y; float *samples; if (!meshGetHeights(mesh, &heights, &columns, &rows, &sizeX, &sizeY, &sizeZ)) { return nullptr; } n = SDL_max(columns + 1, rows + 1); if (n & 1) { n++; } n = SDL_max(n, 4); samples = (float *)SDL_malloc(sizeof(float) * (size_t)n * (size_t)n); if (samples == nullptr) { utilDie("Out of memory building a height field."); } for (y = 0; y < n; y++) { for (x = 0; x < n; x++) { // Bilinear from the mesh's grid; Jolt's row index runs along +Z like the mesh's. float u = (float)x / (float)(n - 1) * (float)columns; float v = (float)y / (float)(n - 1) * (float)rows; int32_t x0 = SDL_min((int32_t)u, columns - 1); int32_t y0 = SDL_min((int32_t)v, rows - 1); float fx = u - (float)x0; float fy = v - (float)y0; float h = (heights[y0 * (columns + 1) + x0] * (1.0f - fx) + heights[y0 * (columns + 1) + x0 + 1] * fx) * (1.0f - fy) + (heights[(y0 + 1) * (columns + 1) + x0] * (1.0f - fx) + heights[(y0 + 1) * (columns + 1) + x0 + 1] * fx) * fy; samples[y * n + x] = h; } } JPH::HeightFieldShapeSettings settings(samples, JPH::Vec3(-sizeX * scale.x / 2.0f, 0.0f, -sizeZ * scale.z / 2.0f), JPH::Vec3(sizeX * scale.x / (float)(n - 1), sizeY * scale.y, sizeZ * scale.z / (float)(n - 1)), (JPH::uint32)n); JPH::ShapeSettings::ShapeResult result = settings.Create(); SDL_free(samples); if (result.HasError()) { utilTrace("Physics: height field: %s", result.GetError().c_str()); return nullptr; } return result.Get(); } // 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 _buildMeshShape(int32_t node, ShapeTypeE shape, Vec3T position, QuatT rotation) { JPH::Array 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, shape, 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: points for a hull, // vertices and triangles for a mesh. void _collectGeometry(int32_t node, const Mat4T *toBody, ShapeTypeE shape, JPH::Array &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])); if (shape == SHAPE_HULL) { points.push_back(JPH::Vec3(v.x, v.y, v.z)); } else { vertices.push_back(JPH::Float3(v.x, v.y, v.z)); } } if (shape != SHAPE_HULL) { 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, shape, points, triangles, vertices); } } // Takes a body out of the world (a no-op when it is not in it) and destroys it. void _destroyBody(JPH::BodyID &id) { JPH::BodyInterface &bodies = _world->system->GetBodyInterface(); bodies.RemoveBody(id); bodies.DestroyBody(id); id = JPH::BodyID(); } // The record for a node's body, or NULL. BodyRecordT *_find(int32_t node) { return (_world == nullptr) ? nullptr : _findRecord(_world->bodyOfNode, _world->bodies, node, _release); } // The record a node maps to in one of the tables, or NULL. A record whose node was deleted (or // whose handle was reused) is released on the way, so a stale handle never reaches its previous // owner's record; a mapping that outlived its record is dropped. template RecordT *_findRecord(std::vector &index, RecordT *table, int32_t node, void (*release)(RecordT *)) { int32_t slot = _indexGet(index, node); RecordT *record; if (slot == NO_HANDLE) { return nullptr; } record = &table[slot]; if (!record->used) { _indexSet(index, node, NO_HANDLE); return nullptr; } if (_nodeStale(node, record->generation)) { release(record); return nullptr; } return record; } 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); } // The slot a node maps to in one of the tables, or NO_HANDLE. int32_t _indexGet(const std::vector &index, int32_t node) { if ((node < 0) || ((size_t)node >= index.size())) { return NO_HANDLE; } return index[(size_t)node]; } // Maps a node to a slot (NO_HANDLE unmaps it), growing the index to reach it. void _indexSet(std::vector &index, int32_t node, int32_t slot) { if (node < 0) { return; } if ((size_t)node >= index.size()) { if (slot == NO_HANDLE) { return; } index.resize((size_t)node + 1, NO_HANDLE); } index[(size_t)node] = slot; } // Whether a node carries a player, from the index alone: Jolt's job threads ask this mid-step, // where the scene must not be read and nothing may be released (the sweep before the step // guarantees no record is stale in that window anyway). bool _isPlayer(int32_t node) { return _indexGet(_world->playerOfNode, node) != NO_HANDLE; } // Whether a record's node was deleted (or its handle reused) since the record took it. bool _nodeStale(int32_t node, uint32_t generation) { return !nodeValid(node) || (nodeGetGeneration(node) != generation); } // Takes the body out of the world (its vehicle and joints first) and frees its slot. void _release(BodyRecordT *record) { VehicleRecordT *vehicle = _findVehicle(record->node); int32_t x; if (vehicle != nullptr) { _releaseVehicle(vehicle); } for (x = 0; x < (int32_t)_world->joints.size(); x++) { if (_world->joints[(size_t)x].used && ((_world->joints[(size_t)x].nodeA == record->node) || (_world->joints[(size_t)x].nodeB == record->node))) { jointDelete(x); } } _world->contacts->forget(record->node, true); _indexSet(_world->bodyOfNode, record->node, NO_HANDLE); _destroyBody(record->id); memset(record, 0, sizeof(*record)); record->node = NO_HANDLE; } // Releases every record whose node is gone, whatever its state: the step loops skip disabled // bodies and players and inactive ragdolls, so this is where those are found. Bodies go first, // since a body's release takes its vehicle and joints with it. void _releaseDead(void) { _releaseStale(_world->bodies, _world->bodyCount, _release); _releaseStale(_world->players, _world->playerCount, _releasePlayer); _releaseStale(_world->ragdolls, _world->ragdollCount, _releaseRagdoll); _releaseStale(_world->softs, _world->softCount, _releaseSoft); _releaseStale(_world->vehicles, _world->vehicleCount, _releaseVehicle); } // The dead-node sweep over one table. template void _releaseStale(RecordT *table, int32_t count, void (*release)(RecordT *)) { int32_t x; for (x = 0; x < count; x++) { if (table[x].used && _nodeStale(table[x].node, table[x].generation)) { release(&table[x]); } } } // Triggers are not solid for a player: the engine tests players against them itself. bool PlayerListenerT::OnContactValidate(const JPH::CharacterVirtual *character, const JPH::CharacterContact &contact) { (void)character; return !contact.mIsSensorB; } void PlayerListenerT::OnContactAdded(const JPH::CharacterVirtual *character, const JPH::CharacterContact &contact, JPH::CharacterContactSettings &settings) { PhysicsEventT event; (void)settings; event.type = PHYSICS_EVENT_COLLISION; event.nodeA = (int32_t)(uint32_t)character->GetUserData(); event.nodeB = (int32_t)(uint32_t)contact.mUserData; event.point = _toVec3(JPH::Vec3(contact.mPosition)); event.speed = fabsf((character->GetLinearVelocity() - contact.mLinearVelocity).Dot(contact.mContactNormal)); _world->contacts->push(event); } // The collision shape for a body or a player, scaled by the node. JPH::RefConst _buildShape(int32_t node, BodyTypeE type, ShapeTypeE shape, float a, float b, float c, Vec3T position, QuatT rotation, Vec3T scale) { float radius; float height; switch (shape) { case SHAPE_BOX: return 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); case SHAPE_SPHERE: return new JPH::SphereShape(SDL_max(a * SDL_max(scale.x, SDL_max(scale.y, scale.z)), MIN_DIMENSION)); case SHAPE_CAPSULE: radius = SDL_max(a * SDL_max(scale.x, scale.z), MIN_DIMENSION); height = SDL_max(b * scale.y, MIN_DIMENSION); return new JPH::CapsuleShape(SDL_max(height / 2.0f - radius, MIN_DIMENSION), radius); case SHAPE_CYLINDER: radius = SDL_max(a * SDL_max(scale.x, scale.z), MIN_DIMENSION); height = SDL_max(b * scale.y, MIN_DIMENSION); return new JPH::CylinderShape(height / 2.0f, radius); 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 nullptr; } if ((shape == SHAPE_MESH) && (nodeGetMesh(node) != NO_HANDLE)) { // A heightmap mesh gets Jolt's height field, far cheaper than its triangles. JPH::RefConst field = _buildHeightField(nodeGetMesh(node), scale); if (field != nullptr) { return field; } } return _buildMeshShape(node, shape, position, rotation); default: utilTrace("Physics: unknown shape %d.", (int32_t)shape); return nullptr; } } PlayerRecordT *_findPlayer(int32_t node) { return (_world == nullptr) ? nullptr : _findRecord(_world->playerOfNode, _world->players, node, _releasePlayer); } // Which way is up for players: against gravity (a 2D game's gravity points down the screen, +Y). JPH::Vec3 _playerUp(void) { JPH::Vec3 gravity = _world->system->GetGravity(); if (gravity.LengthSq() < ZERO_GRAVITY_SQ) { return JPH::Vec3::sAxisY(); } return -gravity.Normalized(); } void _releasePlayer(PlayerRecordT *record) { if (record->used) { _indexSet(_world->playerOfNode, record->node, NO_HANDLE); } record->character = nullptr; record->node = NO_HANDLE; record->generation = 0; record->intent = vec3(0.0f, 0.0f, 0.0f); record->jumpSpeed = 0.0f; record->gravityScale = 1.0f; record->stepHeight = DEFAULT_STEP_HEIGHT; record->height = 0.0f; record->sinkSpeed = DEFAULT_SINK_SPEED; record->swimDrag = DEFAULT_SWIM_DRAG; record->swimming = false; record->swimCurrent = JPH::Vec3::sZero(); record->insideCount = 0; record->enabled = false; record->used = false; } // Moves every player by its intent, gravity and pending jump for one fixed step, before the // world steps so platforms carry it and the bodies it shoves feel it this step. void _stepPlayers(float dt) { JPH::Vec3 gravity = _world->system->GetGravity(); JPH::Vec3 up = _playerUp(); int32_t x; for (x = 0; x < _world->playerCount; x++) { PlayerRecordT *record = &_world->players[x]; JPH::Vec3 velocity; JPH::Vec3 scaledGravity; float vertical; bool onGround; JPH::CharacterVirtual::ExtendedUpdateSettings settings; if (!record->used || !record->enabled) { continue; } scaledGravity = gravity * record->gravityScale; record->character->SetUp(up); velocity = record->character->GetLinearVelocity(); vertical = (velocity - record->character->GetGroundVelocity()).Dot(up); onGround = record->character->GetGroundState() == JPH::CharacterBase::EGroundState::OnGround; record->swimming = _underWater(record->character->GetPosition() + up * (record->height * 0.5f), &record->swimCurrent); if (record->swimming) { // Afloat: the script steers in three axes and the water drags it toward that; with no // vertical intent it sinks slowly, and the current carries it. JPH::Vec3 intent = JPH::Vec3(record->intent.x, record->intent.y, _world->planar ? 0.0f : record->intent.z); JPH::Vec3 target = intent - up * intent.Dot(up) + up * ((intent.Dot(up) != 0.0f) ? intent.Dot(up) : -record->sinkSpeed) + record->swimCurrent; velocity = velocity + (target - velocity) * SDL_min(1.0f, record->swimDrag * dt); } else { if (onGround && (vertical < STANDING_VERTICAL_SPEED)) { // Standing: ride the ground. velocity = record->character->GetGroundVelocity(); } else { // Airborne (or on a slope too steep): keep only the vertical part, and keep falling. velocity = up * velocity.Dot(up) + scaledGravity * dt; } // A jump granted with ground underfoot is taken even as a platform lifts the player. if (onGround && (record->jumpSpeed > 0.0f)) { velocity += up * record->jumpSpeed; } } if (!record->swimming) { velocity += JPH::Vec3(record->intent.x, 0.0f, _world->planar ? 0.0f : record->intent.z); } if (_world->planar) { velocity.SetZ(0.0f); } record->jumpSpeed = 0.0f; record->character->SetLinearVelocity(velocity); settings.mWalkStairsStepUp = up * record->stepHeight; settings.mStickToFloorStepDown = -up * SDL_max(record->stepHeight, record->extent * STICK_PER_EXTENT); settings.mWalkStairsMinStepForward = record->extent * PADDING_PER_EXTENT; settings.mWalkStairsStepForwardTest = record->extent * STEP_TEST_PER_EXTENT; record->character->ExtendedUpdate(dt, scaledGravity, settings, _world->system->GetDefaultBroadPhaseLayerFilter(LAYER_MOVING), _world->system->GetDefaultLayerFilter(LAYER_MOVING), {}, {}, *_world->tempAllocator); if (_world->planar) { JPH::RVec3 where = record->character->GetPosition(); where.SetZ(0.0); record->character->SetPosition(where); } } } SoftRecordT *_findSoft(int32_t node) { return (_world == nullptr) ? nullptr : _findRecord(_world->softOfNode, _world->softs, node, _releaseSoft); } void _releaseSoftBody(SoftRecordT *record) { if (!record->body.IsInvalid()) { _destroyBody(record->body); } } // A rope's tube mesh is the engine's own and goes with it; cloth and pressure bodies use the // node's mesh, which stays. void _releaseSoft(SoftRecordT *record) { _releaseSoftBody(record); if (record->kind == SOFT_ROPE) { meshDelete(record->mesh); } SDL_free(record->meshToSoft); SDL_free(record->positions); SDL_free(record->meshPositions); _resetSoft(record); } void _resetSoft(SoftRecordT *record) { if (record->used) { _indexSet(_world->softOfNode, record->node, NO_HANDLE); } record->shared = nullptr; record->body = JPH::BodyID(); record->node = NO_HANDLE; record->generation = 0; record->kind = SOFT_CLOTH; record->mesh = -1; record->meshToSoft = nullptr; record->positions = nullptr; record->count = 0; record->meshPositions = nullptr; record->meshVertexCount = 0; record->pinCount = 0; record->stretch = DEFAULT_SOFT_STRETCH; record->bend = DEFAULT_SOFT_BEND; record->pressure = 0.0f; record->mass = DEFAULT_SOFT_MASS; record->damping = DEFAULT_SOFT_DAMPING; record->ropeRadius = 0.0f; record->used = false; } // The particle nearest a world point. int32_t _nearestSoftVertex(const SoftRecordT *record, Vec3T point) { int32_t best = -1; float bestDist = 0.0f; int32_t x; for (x = 0; x < record->count; x++) { float dx = record->positions[x * 3] - point.x; float dy = record->positions[x * 3 + 1] - point.y; float dz = record->positions[x * 3 + 2] - point.z; float d = dx * dx + dy * dy + dz * dz; if ((best < 0) || (d < bestDist)) { best = x; bestDist = d; } } return best; } // (Re)makes the Jolt body from the record's particles, constraints and pins: particles in // world space with the body itself pinned at the origin, so what Jolt gives back needs no // transform. Cloth and pressure bodies get Jolt's edge, shear and bend constraints from their // faces; ropes get an edge per segment. bool _buildSoft(SoftRecordT *record) { JPH::BodyInterface &bodies = _world->system->GetBodyInterface(); int32_t x; float invMass = _softInvMass(record); float stretch = STRETCH_COMPLIANCE * (1.0f - SDL_clamp(record->stretch, 0.0f, 1.0f)); float bend = BEND_COMPLIANCE * (1.0f - SDL_clamp(record->bend, 0.0f, 1.0f)); _releaseSoftBody(record); record->shared = new JPH::SoftBodySharedSettings(); for (x = 0; x < record->count; x++) { record->shared->mVertices.push_back(JPH::SoftBodySharedSettings::Vertex(JPH::Float3(record->positions[x * 3], record->positions[x * 3 + 1], record->positions[x * 3 + 2]), JPH::Float3(0.0f, 0.0f, 0.0f), invMass)); } for (x = 0; x < record->pinCount; x++) { record->shared->mVertices[(size_t)record->pins[x].vertex].mInvMass = 0.0f; } if (record->kind == SOFT_ROPE) { for (x = 0; x + 1 < record->count; x++) { record->shared->mEdgeConstraints.push_back(JPH::SoftBodySharedSettings::Edge((JPH::uint32)x, (JPH::uint32)(x + 1), stretch)); } record->shared->CalculateEdgeLengths(); } else { const uint32_t *indices; int32_t indexCount; int32_t vertexCount; const float *unused; JPH::Array attributes; if (!meshGetGeometry(record->mesh, &unused, &vertexCount, &indices, &indexCount) || (vertexCount != record->meshVertexCount)) { utilTrace("Physics: the mesh under soft body node %d is gone.", record->node); return false; } for (x = 0; x + 2 < indexCount; x += 3) { JPH::SoftBodySharedSettings::Face face; face.mVertex[0] = (JPH::uint32)record->meshToSoft[indices[x]]; face.mVertex[1] = (JPH::uint32)record->meshToSoft[indices[x + 1]]; face.mVertex[2] = (JPH::uint32)record->meshToSoft[indices[x + 2]]; if (!face.IsDegenerate()) { record->shared->AddFace(face); } } attributes.resize((size_t)record->count, JPH::SoftBodySharedSettings::VertexAttributes(stretch, stretch, bend)); record->shared->CreateConstraints(attributes.data(), (JPH::uint)attributes.size(), JPH::SoftBodySharedSettings::EBendType::Distance); } record->shared->Optimize(); { JPH::SoftBodyCreationSettings settings(record->shared, JPH::RVec3::sZero(), JPH::Quat::sIdentity(), LAYER_MOVING); JPH::Body *body; settings.mUserData = (JPH::uint64)(uint32_t)record->node; settings.mUpdatePosition = false; settings.mPressure = record->pressure; settings.mLinearDamping = record->damping; settings.mNumIterations = SOFT_ITERATIONS; settings.mFacesDoubleSided = (record->kind == SOFT_CLOTH); settings.mVertexRadius = (record->kind == SOFT_ROPE) ? record->ropeRadius : SOFT_VERTEX_RADIUS; settings.mFriction = DEFAULT_FRICTION; body = bodies.CreateSoftBody(settings); if (body == nullptr) { utilTrace("Physics: unable to create a soft body (too many?)."); return false; } record->body = body->GetID(); bodies.AddBody(record->body, JPH::EActivation::Activate); } return true; } // Pins that follow nodes are dragged to them before every step. void _pinSoft(void) { int32_t x; int32_t p; for (x = 0; x < _world->softCount; x++) { SoftRecordT *record = &_world->softs[x]; bool any = false; if (!record->used || record->body.IsInvalid()) { continue; } for (p = 0; p < record->pinCount; p++) { any = any || (record->pins[p].follow >= 0); } if (!any) { continue; } { JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->body); if (!lock.Succeeded()) { continue; } JPH::SoftBodyMotionProperties *motion = static_cast(lock.GetBody().GetMotionProperties()); for (p = 0; p < record->pinCount; p++) { if ((record->pins[p].follow >= 0) && nodeValid(record->pins[p].follow)) { Vec3T where = nodeGetWorldPosition(record->pins[p].follow); JPH::SoftBodyVertex &vertex = motion->GetVertex((JPH::uint)record->pins[p].vertex); vertex.mPosition = JPH::Vec3(where.x, where.y, where.z); vertex.mVelocity = JPH::Vec3::sZero(); vertex.mInvMass = 0.0f; } } } // Outside the lock: activating takes locks of its own. _world->system->GetBodyInterface().ActivateBody(record->body); } } // A tube round a rope's particles: ROPE_SIDES vertices per particle, framed by the segment. void _ropeMesh(const SoftRecordT *record, Vec3T *out) { int32_t x; int32_t side; for (x = 0; x < record->count; x++) { Vec3T here = vec3(record->positions[x * 3], record->positions[x * 3 + 1], record->positions[x * 3 + 2]); Vec3T next = (x + 1 < record->count) ? vec3(record->positions[(x + 1) * 3], record->positions[(x + 1) * 3 + 1], record->positions[(x + 1) * 3 + 2]) : here; Vec3T before = (x > 0) ? vec3(record->positions[(x - 1) * 3], record->positions[(x - 1) * 3 + 1], record->positions[(x - 1) * 3 + 2]) : here; Vec3T tangent = vec3Subtract(next, before); Vec3T helper; Vec3T u; Vec3T v; tangent = (vec3Length(tangent) > 1e-6f) ? vec3Normalize(tangent) : vec3(0.0f, 1.0f, 0.0f); helper = (fabsf(tangent.y) < 0.9f) ? vec3(0.0f, 1.0f, 0.0f) : vec3(1.0f, 0.0f, 0.0f); u = vec3Normalize(vec3Cross(tangent, helper)); v = vec3Cross(tangent, u); for (side = 0; side < ROPE_SIDES; side++) { float angle = (float)side / (float)ROPE_SIDES * 2.0f * SDL_PI_F; out[x * ROPE_SIDES + side] = vec3Add(here, vec3Add(vec3Scale(u, SDL_cosf(angle) * record->ropeRadius), vec3Scale(v, SDL_sinf(angle) * record->ropeRadius))); } } } // Every particle's inverse mass, from the body's mass shared out evenly. float _softInvMass(const SoftRecordT *record) { return (float)record->count / SDL_max(record->mass, MIN_MASS); } // Writes the particles' inverse masses into the live body (pins stay held) and wakes it. void _softSetMasses(SoftRecordT *record) { float invMass = _softInvMass(record); int32_t v; if (record->body.IsInvalid()) { return; } { JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->body); if (lock.Succeeded()) { JPH::SoftBodyMotionProperties *motion = static_cast(lock.GetBody().GetMotionProperties()); for (v = 0; v < record->count; v++) { motion->GetVertex((JPH::uint)v).mInvMass = invMass; } for (v = 0; v < record->pinCount; v++) { motion->GetVertex((JPH::uint)record->pins[v].vertex).mInvMass = 0.0f; } } } // Outside the lock: activating takes locks of its own. _world->system->GetBodyInterface().ActivateBody(record->body); } // After the steps: particles back from Jolt, then the mesh, in the node's own space. void _writeSoft(void) { int32_t x; int32_t v; for (x = 0; x < _world->softCount; x++) { SoftRecordT *record = &_world->softs[x]; Vec3T position; QuatT rotation; Vec3T scale; QuatT inverse; if (!record->used) { continue; } if (nodeGetMesh(record->node) != record->mesh) { _releaseSoft(record); continue; } if (record->body.IsInvalid()) { continue; } { JPH::BodyLockRead lock(_world->system->GetBodyLockInterface(), record->body); if (!lock.Succeeded()) { continue; } const JPH::SoftBodyMotionProperties *motion = static_cast(lock.GetBody().GetMotionProperties()); JPH::RVec3 origin = lock.GetBody().GetPosition(); for (v = 0; v < record->count; v++) { JPH::Vec3 p = JPH::Vec3(origin) + motion->GetVertex((JPH::uint)v).mPosition; record->positions[v * 3] = p.GetX(); record->positions[v * 3 + 1] = p.GetY(); record->positions[v * 3 + 2] = p.GetZ(); } } // World particles to mesh vertices in the node's frame. nodeGetWorldTransform(record->node, &position, &rotation, &scale); inverse = quatInverse(rotation); if (record->kind == SOFT_ROPE) { _ropeMesh(record, (Vec3T *)record->meshPositions); } for (v = 0; v < record->meshVertexCount; v++) { Vec3T world = (record->kind == SOFT_ROPE) ? ((Vec3T *)record->meshPositions)[v] : vec3(record->positions[record->meshToSoft[v] * 3], record->positions[record->meshToSoft[v] * 3 + 1], record->positions[record->meshToSoft[v] * 3 + 2]); Vec3T local = quatRotate(inverse, vec3Subtract(world, position)); record->meshPositions[v * 3] = local.x / SDL_max(scale.x, MIN_SCALE); record->meshPositions[v * 3 + 1] = local.y / SDL_max(scale.y, MIN_SCALE); record->meshPositions[v * 3 + 2] = local.z / SDL_max(scale.z, MIN_SCALE); } meshSetPositions(record->mesh, record->meshPositions); } } RagdollRecordT *_findRagdoll(int32_t node) { return (_world == nullptr) ? nullptr : _findRecord(_world->ragdollOfNode, _world->ragdolls, node, _releaseRagdoll); } // The first node under (or at) node that carries a skin. int32_t _findSkinned(int32_t node) { int32_t x; int32_t found; if (nodeGetSkinJoints(node, nullptr) > 0) { return node; } for (x = 0; x < nodeGetChildCount(node); x++) { found = _findSkinned(nodeGetChild(node, x)); if (found >= 0) { return found; } } return -1; } int32_t _ragdollPartOf(RagdollRecordT *record, int32_t joint) { int32_t x; for (x = 0; x < record->partCount; x++) { if (record->parts[x].joint == joint) { return x; } } return -1; } void _releaseRagdollBodies(RagdollRecordT *record) { int32_t x; for (x = 0; x < record->partCount; x++) { RagdollPartT *part = &record->parts[x]; if (part->constraint != nullptr) { _world->system->RemoveConstraint(part->constraint); part->constraint = nullptr; } } for (x = 0; x < record->partCount; x++) { RagdollPartT *part = &record->parts[x]; if (!part->body.IsInvalid()) { _destroyBody(part->body); } } record->active = false; } void _releaseRagdoll(RagdollRecordT *record) { _releaseRagdollBodies(record); _resetRagdoll(record); } void _resetRagdoll(RagdollRecordT *record) { int32_t x; if (record->used) { _indexSet(_world->ragdollOfNode, record->node, NO_HANDLE); } for (x = 0; x < MAX_RAGDOLL_PARTS; x++) { record->parts[x].constraint = nullptr; record->parts[x].body = JPH::BodyID(); } record->filter = nullptr; record->node = NO_HANDLE; record->generation = 0; record->skinned = -1; record->partCount = 0; record->strength = 0.0f; record->strengthChanged = false; record->active = false; record->used = false; } // Motors toward the pose the ragdoll had when it was activated, as strong as asked. void _steerRagdolls(void) { int32_t x; int32_t p; for (x = 0; x < _world->ragdollCount; x++) { RagdollRecordT *record = &_world->ragdolls[x]; if (!record->used || !record->active || !record->strengthChanged) { continue; } record->strengthChanged = false; for (p = 0; p < record->partCount; p++) { RagdollPartT *part = &record->parts[p]; JPH::SwingTwistConstraint *constraint; if (part->constraint == nullptr) { continue; } constraint = static_cast(part->constraint.GetPtr()); if (record->strength > 0.0f) { constraint->GetSwingMotorSettings().mMaxTorqueLimit = record->strength; constraint->GetSwingMotorSettings().mMinTorqueLimit = -record->strength; constraint->GetTwistMotorSettings().mMaxTorqueLimit = record->strength; constraint->GetTwistMotorSettings().mMinTorqueLimit = -record->strength; constraint->SetTargetOrientationBS(part->rest); constraint->SetSwingMotorState(JPH::EMotorState::Position); constraint->SetTwistMotorState(JPH::EMotorState::Position); } else { constraint->SetSwingMotorState(JPH::EMotorState::Off); constraint->SetTwistMotorState(JPH::EMotorState::Off); } } } } // The bones' bodies drive their joint nodes: world transforms from the bodies, then locals // against each joint's parent, parts first so children read their parents' new poses. void _writeRagdolls(void) { JPH::BodyInterface &bodies = _world->system->GetBodyInterface(); int32_t x; int32_t p; for (x = 0; x < _world->ragdollCount; x++) { RagdollRecordT *record = &_world->ragdolls[x]; JPH::RVec3 worldPosition[MAX_RAGDOLL_PARTS]; JPH::Quat worldRotation[MAX_RAGDOLL_PARTS]; if (!record->used || !record->active) { continue; } for (p = 0; p < record->partCount; p++) { RagdollPartT *part = &record->parts[p]; JPH::RVec3 position; JPH::Quat rotation; bodies.GetPositionAndRotation(part->body, position, rotation); worldPosition[p] = position + rotation * part->offsetPosition; worldRotation[p] = rotation * part->offsetRotation; } for (p = 0; p < record->partCount; p++) { RagdollPartT *part = &record->parts[p]; int32_t parentNode = nodeGetParent(part->joint); int32_t parentPart = (parentNode >= 0) ? _ragdollPartOf(record, parentNode) : -1; JPH::RVec3 parentPosition; JPH::Quat parentRotation; JPH::Vec3 parentScale; JPH::Vec3 local; JPH::Quat localRotation; if (parentPart >= 0) { parentPosition = worldPosition[parentPart]; parentRotation = worldRotation[parentPart]; parentScale = record->parts[parentPart].scale; } else if (parentNode >= 0) { Vec3T position; QuatT rotation; Vec3T scale; nodeGetWorldTransform(parentNode, &position, &rotation, &scale); parentPosition = JPH::RVec3(position.x, position.y, position.z); parentRotation = _fromQuat(rotation); parentScale = JPH::Vec3(scale.x, scale.y, scale.z); } else { parentPosition = JPH::RVec3::sZero(); parentRotation = JPH::Quat::sIdentity(); parentScale = JPH::Vec3::sOne(); } local = parentRotation.Conjugated() * JPH::Vec3(worldPosition[p] - parentPosition); local = JPH::Vec3(local.GetX() / SDL_max(parentScale.GetX(), MIN_SCALE), local.GetY() / SDL_max(parentScale.GetY(), MIN_SCALE), local.GetZ() / SDL_max(parentScale.GetZ(), MIN_SCALE)); localRotation = parentRotation.Conjugated() * worldRotation[p]; nodeSetPosition(part->joint, vec3(local.GetX(), local.GetY(), local.GetZ())); nodeSetRotation(part->joint, _toQuat(localRotation)); } } } VehicleRecordT *_findVehicle(int32_t node) { return (_world == nullptr) ? nullptr : _findRecord(_world->vehicleOfNode, _world->vehicles, node, _releaseVehicle); } void _releaseVehicle(VehicleRecordT *record) { if (record->constraint != nullptr) { _world->system->RemoveStepListener(record->constraint); _world->system->RemoveConstraint(record->constraint); } record->constraint = nullptr; record->tester = nullptr; _resetVehicle(record); } void _resetVehicle(VehicleRecordT *record) { if (record->used) { _indexSet(_world->vehicleOfNode, record->node, NO_HANDLE); } record->node = NO_HANDLE; record->generation = 0; record->kind = VEHICLE_CAR; record->wheelCount = 0; record->maxTorque = DEFAULT_ENGINE_TORQUE; record->maxRpm = DEFAULT_ENGINE_MAX_RPM; record->minRpm = VEHICLE_DEFAULT_MIN_RPM; record->gearCount = 0; record->reverseGear = -VEHICLE_DEFAULT_REVERSE_GEAR; record->automatic = true; record->suspensionHz = DEFAULT_SUSPENSION_HZ; record->suspensionDamping = DEFAULT_SUSPENSION_DAMPING; record->maxSteer = DEFAULT_STEER_DEGREES; record->brakeTorque = DEFAULT_BRAKE_TORQUE; record->handBrakeTorque = DEFAULT_HANDBRAKE_TORQUE; record->antiRoll = 0.0f; record->inputForward = 0.0f; record->inputRight = 0.0f; record->inputBrake = 0.0f; record->inputHandBrake = 0.0f; record->thrust = DEFAULT_THRUST; record->thrustPoint = vec3(0.0f, DEFAULT_THRUST_Y, DEFAULT_THRUST_Z); record->rudder = DEFAULT_RUDDER; record->dirty = false; record->used = false; } // Engine and gearbox settings from the recipe (a script that set no gears keeps Jolt's). void _setDrivetrain(const VehicleRecordT *record, JPH::VehicleEngineSettings &engine, JPH::VehicleTransmissionSettings &transmission) { int32_t v; engine.mMaxTorque = record->maxTorque; engine.mMaxRPM = record->maxRpm; engine.mMinRPM = record->minRpm; if (record->gearCount > 0) { transmission.mGearRatios.clear(); for (v = 0; v < record->gearCount; v++) { transmission.mGearRatios.push_back(record->gears[v]); } transmission.mReverseGearRatios.clear(); transmission.mReverseGearRatios.push_back(record->reverseGear); } transmission.mMode = record->automatic ? JPH::ETransmissionMode::Auto : JPH::ETransmissionMode::Manual; } // Buoyancy, drag and current for every dynamic body inside a water volume, and the surfaces // players and boats test against this step. A volume's surface is the top of its box. void _applyWater(float dt) { JPH::BodyInterface &bodies = _world->system->GetBodyInterface(); JPH::Vec3 gravity = _world->system->GetGravity(); int32_t x; size_t h; _world->waterCount = 0; for (x = 0; x < _world->bodyCount; x++) { BodyRecordT *record = &_world->bodies[x]; WaterSurfaceT surface; JPH::AllHitCollisionCollector hits; if (!record->used || !record->water || !record->enabled) { continue; } { JPH::TransformedShape shape = bodies.GetTransformedShape(record->id); JPH::AABox local = bodies.GetShape(record->id)->GetLocalBounds(); JPH::Quat rotation = bodies.GetRotation(record->id); surface.box = shape.GetWorldSpaceBounds(); surface.normal = rotation * JPH::Vec3::sAxisY(); surface.position = bodies.GetPosition(record->id) + rotation * JPH::Vec3(0.0f, local.mMax.GetY(), 0.0f); surface.current = JPH::Vec3(record->current.x, record->current.y, record->current.z); } if (_world->waterCount < MAX_WATERS) { _world->waters[_world->waterCount++] = surface; } _world->system->GetBroadPhaseQuery().CollideAABox(surface.box, hits, _world->system->GetDefaultBroadPhaseLayerFilter(LAYER_MOVING), _world->system->GetDefaultLayerFilter(LAYER_MOVING)); for (h = 0; h < hits.mHits.size(); h++) { JPH::BodyID id = hits.mHits[h]; BodyRecordT *other; int32_t node; if (id == record->id) { continue; } { // Rigid, dynamic and solid: soft bodies have no buoyancy (Jolt asserts on them). JPH::BodyLockRead lock(_world->system->GetBodyLockInterface(), id); if (!lock.Succeeded() || !lock.GetBody().IsRigidBody() || !lock.GetBody().IsDynamic() || lock.GetBody().IsSensor()) { continue; } node = (int32_t)(uint32_t)lock.GetBody().GetUserData(); } other = _find(node); bodies.ApplyBuoyancyImpulse(id, surface.position, surface.normal, record->waterDensity * ((other != nullptr) ? other->buoyancy : DEFAULT_BUOYANCY), record->waterLinearDrag, record->waterAngularDrag, surface.current, gravity, dt); } } } // Whether a world point is below a water surface this step; hands back the water's current. bool _underWater(JPH::RVec3Arg point, JPH::Vec3 *current) { int32_t x; for (x = 0; x < _world->waterCount; x++) { WaterSurfaceT *water = &_world->waters[x]; if (water->box.Contains(JPH::Vec3(point)) && (JPH::Vec3(point - water->position).Dot(water->normal) < 0.0f)) { if (current != nullptr) { *current = water->current; } return true; } } return false; } // Builds (or rebuilds) the Jolt constraint from the recipe. The chassis faces -Z with Y up, like // everything else in the scene; wheels sit where their nodes are relative to the chassis. One // try per change: a recipe that cannot be built is left alone until a setting changes. bool _buildVehicle(VehicleRecordT *record) { BodyRecordT *body = _find(record->node); JPH::VehicleConstraintSettings settings; JPH::Vec3 forward = JPH::Vec3(0.0f, 0.0f, -1.0f); JPH::Vec3 up = JPH::Vec3::sAxisY(); JPH::Vec3 left = up.Cross(forward); JPH::Vec3 local[MAX_WHEELS]; float along[MAX_WHEELS]; float meanAlong = 0.0f; float rollInertia = LEAN_INERTIA; int32_t w; int32_t v; record->dirty = false; if ((body == nullptr) || (body->type != BODY_DYNAMIC)) { utilTrace("Physics: vehicle node %d needs a dynamic body as its chassis.", record->node); return false; } if (record->wheelCount < 2) { utilTrace("Physics: vehicle node %d needs at least two wheels.", record->node); return false; } if (record->constraint != nullptr) { _world->system->RemoveStepListener(record->constraint); _world->system->RemoveConstraint(record->constraint); record->constraint = nullptr; } // The chassis' inertia about its nose, for the motorcycle lean spring. { JPH::BodyLockRead lock(_world->system->GetBodyLockInterface(), body->id); if (lock.Succeeded() && (lock.GetBody().GetMotionProperties() != nullptr)) { float inverse = forward.Dot(lock.GetBody().GetMotionProperties()->GetLocalSpaceInverseInertia().Multiply3x3(forward)); if (inverse > MIN_INVERSE_INERTIA) { rollInertia = 1.0f / inverse; } } } // Wheel attachment points in the chassis' frame, as recorded when the wheels were added. for (w = 0; w < record->wheelCount; w++) { local[w] = JPH::Vec3(record->wheels[w].rest.x, record->wheels[w].rest.y, record->wheels[w].rest.z); along[w] = local[w].Dot(forward); meanAlong += along[w] / (float)record->wheelCount; } settings.mUp = up; settings.mForward = forward; settings.mMaxPitchRollAngle = (record->kind == VEHICLE_MOTORCYCLE) ? JPH::DegreesToRadians(VEHICLE_MAX_TILT_DEGREES) : JPH::JPH_PI; for (w = 0; w < record->wheelCount; w++) { WheelRecordT *wheel = &record->wheels[w]; bool front = along[w] > meanAlong + FRONT_EPSILON; bool steered = wheel->steeredSet ? wheel->steered : front; bool driven = wheel->steeredSet ? wheel->driven : (record->kind != VEHICLE_MOTORCYCLE || !front); JPH::WheelSettings *base; if (record->kind == VEHICLE_TANK) { JPH::WheelSettingsTV *tv = new JPH::WheelSettingsTV(); base = tv; } else { JPH::WheelSettingsWV *wv = new JPH::WheelSettingsWV(); wv->mMaxSteerAngle = steered ? JPH::DegreesToRadians(record->maxSteer) : 0.0f; wv->mMaxBrakeTorque = record->brakeTorque; wv->mMaxHandBrakeTorque = front ? 0.0f : record->handBrakeTorque; base = wv; } base->mPosition = local[w]; base->mSuspensionDirection = -up; base->mSteeringAxis = up; base->mWheelUp = up; base->mWheelForward = forward; base->mSuspensionMinLength = wheel->suspension * 0.5f; base->mSuspensionMaxLength = wheel->suspension; base->mSuspensionSpring = JPH::SpringSettings(JPH::ESpringMode::FrequencyAndDamping, record->suspensionHz, record->suspensionDamping); base->mRadius = wheel->radius; base->mWidth = wheel->width; wheel->steered = steered; wheel->driven = driven; settings.mWheels.push_back(base); } // The settings object holds the controller settings from the moment they are made, so an // early return frees them. if (record->kind == VEHICLE_TANK) { JPH::TrackedVehicleControllerSettings *controller = new JPH::TrackedVehicleControllerSettings(); int32_t sides[2] = { 0, 0 }; settings.mController = controller; _setDrivetrain(record, controller->mEngine, controller->mTransmission); for (w = 0; w < record->wheelCount; w++) { int32_t side = (local[w].Dot(left) > 0.0f) ? (int32_t)JPH::ETrackSide::Left : (int32_t)JPH::ETrackSide::Right; if (sides[side] == 0) { controller->mTracks[side].mDrivenWheel = (JPH::uint)w; } controller->mTracks[side].mWheels.push_back((JPH::uint)w); sides[side]++; } if ((sides[0] == 0) || (sides[1] == 0)) { utilTrace("Physics: a tank needs wheels on both sides of node %d.", record->node); return false; } } else { JPH::WheeledVehicleControllerSettings *controller = (record->kind == VEHICLE_MOTORCYCLE) ? new JPH::MotorcycleControllerSettings() : new JPH::WheeledVehicleControllerSettings(); bool paired[MAX_WHEELS]; int32_t axles = 0; settings.mController = controller; if (record->kind == VEHICLE_MOTORCYCLE) { // Jolt's lean spring is tuned for one bike; a lighter or slimmer one flips with it. JPH::MotorcycleControllerSettings *bike = static_cast(controller); bike->mLeanSpringConstant = LEAN_SPRING * rollInertia / LEAN_INERTIA; bike->mLeanSpringDamping = LEAN_DAMPING * rollInertia / LEAN_INERTIA; } _setDrivetrain(record, controller->mEngine, controller->mTransmission); // Driven wheels pair up across the chassis into differentials, one per axle; a lone wheel // (a motorcycle's) is an axle of its own. Anti-roll bars follow the same pairs. for (w = 0; w < record->wheelCount; w++) { paired[w] = false; } for (w = 0; w < record->wheelCount; w++) { JPH::VehicleDifferentialSettings differential; int32_t partner = -1; if (paired[w] || !record->wheels[w].driven) { continue; } for (v = w + 1; v < record->wheelCount; v++) { if (!paired[v] && record->wheels[v].driven && (fabsf(along[v] - along[w]) < record->wheels[w].radius) && ((local[v].Dot(left) > 0.0f) != (local[w].Dot(left) > 0.0f))) { partner = v; break; } } paired[w] = true; if (local[w].Dot(left) > 0.0f) { differential.mLeftWheel = w; differential.mRightWheel = partner; } else { differential.mLeftWheel = partner; differential.mRightWheel = w; } if (partner >= 0) { paired[partner] = true; if (record->antiRoll > 0.0f) { JPH::VehicleAntiRollBar bar; bar.mLeftWheel = differential.mLeftWheel; bar.mRightWheel = differential.mRightWheel; bar.mStiffness = record->antiRoll; settings.mAntiRollBars.push_back(bar); } } controller->mDifferentials.push_back(differential); axles++; } for (v = 0; v < (int32_t)controller->mDifferentials.size(); v++) { controller->mDifferentials[(size_t)v].mEngineTorqueRatio = 1.0f / (float)SDL_max(axles, 1); } if (axles == 0) { utilTrace("Physics: vehicle node %d has no driven wheel.", record->node); return false; } } { JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), body->id); if (!lock.Succeeded()) { return false; } record->constraint = new JPH::VehicleConstraint(lock.GetBody(), settings); } // Wheels feel the ground with a cylinder cast (kerbs and edges); tracks with a ray, which // still finds the ground when a heavy hull has come down on its belly. if (record->kind == VEHICLE_TANK) { record->tester = new JPH::VehicleCollisionTesterRay(LAYER_MOVING, up); } else { record->tester = new JPH::VehicleCollisionTesterCastCylinder(LAYER_MOVING, WHEEL_CAST_RADIUS); } record->constraint->SetVehicleCollisionTester(record->tester); _world->system->AddConstraint(record->constraint); _world->system->AddStepListener(record->constraint); return true; } // Hands each vehicle its driver input for the coming steps, rebuilding stale ones first. void _driveVehicles(void) { JPH::BodyInterface &bodies = _world->system->GetBodyInterface(); int32_t x; for (x = 0; x < _world->vehicleCount; x++) { VehicleRecordT *record = &_world->vehicles[x]; BodyRecordT *body; if (!record->used) { continue; } // _find checks the node and its generation, which the vehicle shares with its body. body = _find(record->node); if (body == nullptr) { _releaseVehicle(record); continue; } if (record->kind == VEHICLE_BOAT) { JPH::Quat rotation = bodies.GetRotation(body->id); JPH::RVec3 point = bodies.GetPosition(body->id) + rotation * JPH::Vec3(record->thrustPoint.x, record->thrustPoint.y, record->thrustPoint.z); JPH::Vec3 forward = rotation * JPH::Vec3(0.0f, 0.0f, -1.0f); JPH::Vec3 up = rotation * JPH::Vec3::sAxisY(); float speed = bodies.GetLinearVelocity(body->id).Dot(forward); record->dirty = false; if (_underWater(point, nullptr)) { if (record->inputForward != 0.0f) { bodies.AddForce(body->id, forward * (record->thrust * record->inputForward), point); } if (record->inputRight != 0.0f) { // The rudder bites with speed. bodies.AddTorque(body->id, up * (-record->inputRight * record->rudder * SDL_clamp(fabsf(speed) / RUDDER_FULL_SPEED, RUDDER_MIN_BITE, 1.0f) * ((speed < 0.0f) ? -1.0f : 1.0f))); } if (record->inputBrake > 0.0f) { bodies.AddForce(body->id, -bodies.GetLinearVelocity(body->id) * (record->thrust * record->inputBrake * BOAT_BRAKE_RATIO)); } } continue; } if (record->dirty && !_buildVehicle(record)) { continue; } if (record->constraint == nullptr) { continue; } if (record->kind == VEHICLE_TANK) { JPH::TrackedVehicleController *controller = static_cast(record->constraint->GetController()); float forward = record->inputForward; float leftRatio = 1.0f; float rightRatio = 1.0f; if ((forward == 0.0f) && (record->inputRight != 0.0f)) { // Turning on the spot: the tracks run against each other, gently, or a heavy // hull hops off the ground. forward = fabsf(record->inputRight) * TANK_PIVOT_THROTTLE; leftRatio = (record->inputRight > 0.0f) ? 1.0f : -1.0f; rightRatio = -leftRatio; } else if (record->inputRight > 0.0f) { rightRatio = SDL_max(1.0f - record->inputRight * TANK_TURN_RATIO, TANK_TURN_MIN); } else if (record->inputRight < 0.0f) { leftRatio = SDL_max(1.0f + record->inputRight * TANK_TURN_RATIO, TANK_TURN_MIN); } controller->SetDriverInput(forward, leftRatio, rightRatio, record->inputBrake); } else { JPH::WheeledVehicleController *controller = static_cast(record->constraint->GetController()); controller->SetDriverInput(record->inputForward, record->inputRight, record->inputBrake, record->inputHandBrake); } if ((record->inputForward != 0.0f) || (record->inputRight != 0.0f) || (record->inputBrake != 0.0f)) { bodies.ActivateBody(body->id); } } } // Wheel nodes take the wheel transforms after the step: X is the axle, Y up. void _poseWheels(void) { int32_t x; int32_t w; for (x = 0; x < _world->vehicleCount; x++) { VehicleRecordT *record = &_world->vehicles[x]; if (!record->used || (record->constraint == nullptr)) { continue; } for (w = 0; w < record->wheelCount; w++) { JPH::RMat44 transform; JPH::Quat rotation; JPH::RVec3 position; if (_nodeStale(record->wheels[w].node, record->wheels[w].generation)) { continue; } transform = record->constraint->GetWheelWorldTransform((JPH::uint)w, JPH::Vec3::sAxisX(), JPH::Vec3::sAxisY()); position = transform.GetTranslation(); rotation = transform.GetQuaternion(); nodeSetWorldTransform(record->wheels[w].node, vec3((float)position.GetX(), (float)position.GetY(), (float)position.GetZ()), _toQuat(rotation)); } } } // The triggers a player is in now against those it was in: onTrigger enter and leave. void _playerInside(PlayerRecordT *record, const int32_t *now, int32_t count) { int32_t x; int32_t y; for (x = 0; x < count; x++) { bool had = false; for (y = 0; y < record->insideCount; y++) { had = had || (record->inside[y] == now[x]); } if (!had) { _world->contacts->push(_triggerEvent(PHYSICS_EVENT_ENTER, now[x], record->node)); } } for (y = 0; y < record->insideCount; y++) { bool still = false; for (x = 0; x < count; x++) { still = still || (now[x] == record->inside[y]); } if (!still) { _world->contacts->push(_triggerEvent(PHYSICS_EVENT_LEAVE, record->inside[y], record->node)); } } for (x = 0; x < count; x++) { record->inside[x] = now[x]; } record->insideCount = count; } // Which triggers a player overlaps this frame, reported against the last frame's answer. void _playerTriggers(PlayerRecordT *record) { JPH::AllHitCollisionCollector hits; JPH::AABox box = record->character->GetShape()->GetWorldSpaceBounds(record->character->GetCenterOfMassTransform(), JPH::Vec3::sOne()); int32_t now[MAX_PLAYER_TRIGGERS]; int32_t count = 0; int32_t x; size_t h; _world->system->GetBroadPhaseQuery().CollideAABox(box, hits, _world->system->GetDefaultBroadPhaseLayerFilter(LAYER_MOVING), _world->system->GetDefaultLayerFilter(LAYER_MOVING)); for (h = 0; (h < hits.mHits.size()) && (count < MAX_PLAYER_TRIGGERS); h++) { int32_t node = _triggerNode(hits.mHits[h]); bool seen = false; if (node == NO_HANDLE) { continue; } // The broad phase may name a body more than once. for (x = 0; x < count; x++) { seen = seen || (now[x] == node); } if (!seen) { now[count++] = node; } } _playerInside(record, now, count); } // An enter or leave event for a trigger and what crossed it. PhysicsEventT _triggerEvent(PhysicsEventTypeE type, int32_t trigger, int32_t node) { PhysicsEventT event; event.type = type; event.nodeA = trigger; event.nodeB = node; event.point = vec3(0.0f, 0.0f, 0.0f); event.speed = 0.0f; return event; } // The node of a Jolt body that is a trigger, or NO_HANDLE. int32_t _triggerNode(JPH::BodyID id) { JPH::BodyLockRead lock(_world->system->GetBodyLockInterface(), id); if (!lock.Succeeded() || !lock.GetBody().IsSensor()) { return NO_HANDLE; } return (int32_t)(uint32_t)lock.GetBody().GetUserData(); } // Players drive their nodes' positions; the script owns the rotation. void _writePlayers(void) { int32_t x; Vec3T position; QuatT rotation; Vec3T scale; for (x = 0; x < _world->playerCount; x++) { PlayerRecordT *record = &_world->players[x]; JPH::RVec3 where; if (!record->used || !record->enabled) { continue; } _playerTriggers(record); where = record->character->GetPosition(); nodeGetWorldTransform(record->node, &position, &rotation, &scale); nodeSetWorldTransform(record->node, vec3((float)where.GetX(), (float)where.GetY(), (float)where.GetZ()), rotation); record->intent = vec3(0.0f, 0.0f, 0.0f); } } 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); } } // 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 joltShape; Vec3T position; QuatT rotation; Vec3T scale; int32_t x; if ((_world == nullptr) || !nodeValid(node)) { return false; } bodyDelete(node); playerDelete(node); sceneUpdateTransforms(); nodeGetWorldTransform(node, &position, &rotation, &scale); joltShape = _buildShape(node, type, shape, a, b, c, position, rotation, scale); if (joltShape == nullptr) { return false; } for (x = 0; x < _world->bodyCount; x++) { if (!_world->bodies[x].used) { break; } } if (x == _world->bodyCount) { if (_world->bodyCount == MAX_BODIES) { utilTrace("Physics: no room for another body (%d already).", MAX_BODIES); return false; } _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; record->buoyancy = DEFAULT_BUOYANCY; _indexSet(_world->bodyOfNode, node, x); 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; } bool bodySetBuoyancy(int32_t node, float factor) { BodyRecordT *record = _find(node); if (record == nullptr) { return false; } record->buoyancy = SDL_max(0.0f, factor); return true; } bool bodySetCurrent(int32_t node, Vec3T flow) { BodyRecordT *record = _find(node); if (record == nullptr) { return false; } record->current = flow; 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); } if (!trigger) { _world->contacts->forget(node, false); } 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; } // Fills a static trigger with water: bodies inside float, sink and drift; players swim. bool bodySetWater(int32_t node, float density, float linearDrag, float angularDrag) { BodyRecordT *record = _find(node); if ((record == nullptr) || (record->type != BODY_STATIC)) { utilTrace("Physics: water needs a static body on node %d.", node); return false; } if (!record->trigger) { bodySetTrigger(node, true); } record->water = true; record->waterDensity = SDL_max(0.0f, density); record->waterLinearDrag = SDL_max(0.0f, linearDrag); record->waterAngularDrag = SDL_max(0.0f, angularDrag); return true; } bool jointDelete(int32_t joint) { JointRecordT *record; if (!jointValid(joint)) { return false; } record = &_world->joints[(size_t)joint]; _world->system->RemoveConstraint(record->constraint); record->constraint = nullptr; record->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 constraint; JPH::Vec3 direction; JPH::Vec3 normal; JointRecordT record; 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); record.constraint = constraint; record.type = type; record.nodeA = nodeA; record.nodeB = nodeB; record.used = true; for (x = 0; x < (int32_t)_world->joints.size(); x++) { if (!_world->joints[(size_t)x].used) { _world->joints[(size_t)x] = record; return x; } } _world->joints.push_back(record); 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[(size_t)joint].type == JOINT_HINGE) { static_cast(_world->joints[(size_t)joint].constraint.GetPtr())->SetLimits(JPH::DegreesToRadians(-high), JPH::DegreesToRadians(-low)); return true; } if (_world->joints[(size_t)joint].type == JOINT_SLIDER) { static_cast(_world->joints[(size_t)joint].constraint.GetPtr())->SetLimits(-high, -low); return true; } return false; } bool jointValid(int32_t joint) { return (_world != nullptr) && (joint >= 0) && (joint < (int32_t)_world->joints.size()) && _world->joints[(size_t)joint].used; } bool physicsAvailable(void) { return _world != nullptr; } // Hands the engine up to maximum of the queued events, oldest first, and drops only those: call // until it returns 0 to drain a busy step. int32_t physicsGetEvents(PhysicsEventT *events, int32_t maximum) { int32_t count; int32_t x; if ((_world == nullptr) || (maximum <= 0)) { return 0; } { std::lock_guard guard(_world->contacts->lock); std::vector &queue = _world->contacts->events; count = SDL_min(maximum, (int32_t)queue.size()); for (x = 0; x < count; x++) { events[x] = queue[(size_t)x]; } queue.erase(queue.begin(), queue.begin() + count); } 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; int32_t x; 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->playerListener = new PlayerListenerT(); // The tables are value-initialised (all zero, so nothing is used) and then given their defaults. _world->bodies = new BodyRecordT[MAX_BODIES](); _world->bodyCount = 0; _world->players = new PlayerRecordT[MAX_PLAYERS](); _world->playerCount = MAX_PLAYERS; for (x = 0; x < MAX_PLAYERS; x++) { _releasePlayer(&_world->players[x]); } _world->vehicles = new VehicleRecordT[MAX_VEHICLES](); _world->vehicleCount = MAX_VEHICLES; for (x = 0; x < MAX_VEHICLES; x++) { _resetVehicle(&_world->vehicles[x]); } _world->softs = new SoftRecordT[MAX_SOFT](); _world->softCount = MAX_SOFT; for (x = 0; x < MAX_SOFT; x++) { _resetSoft(&_world->softs[x]); } _world->ragdolls = new RagdollRecordT[MAX_RAGDOLLS](); _world->ragdollCount = MAX_RAGDOLLS; _world->ragdollGroups = 1; for (x = 0; x < MAX_RAGDOLLS; x++) { _resetRagdoll(&_world->ragdolls[x]); } _world->enabled = true; _debugMask = DEBUG_NONE; 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]); } } delete[] _world->bodies; for (x = 0; x < _world->softCount; x++) { if (_world->softs[x].used) { _releaseSoft(&_world->softs[x]); } } delete[] _world->softs; for (x = 0; x < _world->ragdollCount; x++) { if (_world->ragdolls[x].used) { _releaseRagdoll(&_world->ragdolls[x]); } } delete[] _world->ragdolls; for (x = 0; x < _world->vehicleCount; x++) { if (_world->vehicles[x].used) { _releaseVehicle(&_world->vehicles[x]); } } delete[] _world->vehicles; for (x = 0; x < _world->playerCount; x++) { _releasePlayer(&_world->players[x]); } delete[] _world->players; delete _world->playerListener; 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; #ifdef JPH_DEBUG_RENDERER delete _renderer; _renderer = nullptr; #endif } // 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; } } void physicsSetDebug(uint32_t mask) { #ifdef JPH_DEBUG_RENDERER _debugMask = mask; #else _debugMask = DEBUG_NONE; if (mask != DEBUG_NONE) { utilTrace("Physics: this build has no debug renderer."); } #endif } // 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: records whose nodes are gone are released, 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; if (_world == nullptr) { return; } _releaseDead(); now = utilTicksNS(); 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) { _step(); } _drawDebug(); } bool playerDelete(int32_t node) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } _releasePlayer(record); return true; } bool playerExists(int32_t node) { return _findPlayer(node) != nullptr; } // The node stood on, or -1, and the ground normal. int32_t playerGetGround(int32_t node, Vec3T *normal) { PlayerRecordT *record = _findPlayer(node); JPH::Vec3 n; if ((record == nullptr) || !record->character->IsSupported()) { if (normal != nullptr) { *normal = vec3(0.0f, 1.0f, 0.0f); } return -1; } n = record->character->GetGroundNormal(); if (normal != nullptr) { *normal = vec3(n.GetX(), n.GetY(), n.GetZ()); } if (record->character->GetGroundBodyID().IsInvalid()) { return -1; } return (int32_t)(uint32_t)_world->system->GetBodyInterface().GetUserData(record->character->GetGroundBodyID()); } Vec3T playerGetVelocity(int32_t node) { PlayerRecordT *record = _findPlayer(node); JPH::Vec3 v; if (record == nullptr) { return vec3(0.0f, 0.0f, 0.0f); } v = record->character->GetLinearVelocity(); return vec3(v.GetX(), v.GetY(), v.GetZ()); } bool playerIsOnGround(int32_t node) { PlayerRecordT *record = _findPlayer(node); return (record != nullptr) && (record->character->GetGroundState() == JPH::CharacterBase::EGroundState::OnGround); } bool playerIsSwimming(int32_t node) { PlayerRecordT *record = _findPlayer(node); return (record != nullptr) && record->swimming; } // Asks for a jump at the next step; only granted with ground underfoot. bool playerJump(int32_t node, float speed) { PlayerRecordT *record = _findPlayer(node); if ((record == nullptr) || (record->character->GetGroundState() != JPH::CharacterBase::EGroundState::OnGround)) { return false; } record->jumpSpeed = SDL_max(0.0f, speed); return true; } bool playerMove(int32_t node, Vec3T velocity) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->intent = velocity; return true; } // A character on the node, shaped like a body but standing on the node's origin. bool playerNew(int32_t node, ShapeTypeE shape, float a, float b, float c) { PlayerRecordT *record = nullptr; JPH::RefConst joltShape; JPH::RefConst standing; JPH::CharacterVirtualSettings settings; JPH::Shape::ShapeResult lifted; JPH::AABox bounds; Vec3T position; QuatT rotation; Vec3T scale; JPH::Vec3 up; float lift; float extent; int32_t x; if ((_world == nullptr) || !nodeValid(node)) { return false; } if (shape == SHAPE_MESH) { utilTrace("Physics: a player needs a convex shape; use a hull for node %d.", node); return false; } bodyDelete(node); playerDelete(node); sceneUpdateTransforms(); nodeGetWorldTransform(node, &position, &rotation, &scale); joltShape = _buildShape(node, BODY_DYNAMIC, shape, a, b, c, position, rotation, scale); if (joltShape == nullptr) { return false; } for (x = 0; x < _world->playerCount; x++) { if (!_world->players[x].used) { record = &_world->players[x]; break; } } if (record == nullptr) { utilTrace("Physics: no room for another player (%d already).", MAX_PLAYERS); return false; } // Feet at the origin: lift the shape (along up, which is against gravity) by half its height. up = _playerUp(); bounds = joltShape->GetLocalBounds(); lift = (bounds.mMax.GetY() - bounds.mMin.GetY()) / 2.0f; lifted = JPH::RotatedTranslatedShapeSettings(up * lift, JPH::Quat::sIdentity(), joltShape).Create(); if (lifted.HasError()) { utilTrace("Physics: player shape: %s", lifted.GetError().c_str()); return false; } standing = lifted.Get(); settings.mShape = standing; settings.mInnerBodyShape = standing; settings.mInnerBodyLayer = LAYER_MOVING; settings.mMaxSlopeAngle = JPH::DegreesToRadians(DEFAULT_SLOPE_DEGREES); settings.mMaxStrength = DEFAULT_PUSH_STRENGTH; settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces; settings.mEnhancedInternalEdgeRemoval = true; settings.mUp = up; settings.mSupportingVolume = JPH::Plane(up, -SDL_max((bounds.mMax.GetY() - bounds.mMin.GetY()) * 0.25f, MIN_DIMENSION)); // Jolt's tolerances are tuned for a metre-sized capsule; a pixel-sized 2D player needs them // scaled up with it, so they follow the shape's smallest half-extent (0.3 m gives the defaults). extent = SDL_max(MIN_DIMENSION, (bounds.mMax - bounds.mMin).ReduceMin() / 2.0f); settings.mCharacterPadding = extent * PADDING_PER_EXTENT; settings.mPredictiveContactDistance = extent * PREDICTIVE_PER_EXTENT; settings.mCollisionTolerance = extent * TOLERANCE_PER_EXTENT; record->character = new JPH::CharacterVirtual(&settings, JPH::RVec3(position.x, position.y, position.z), JPH::Quat::sIdentity(), (JPH::uint64)(uint32_t)node, _world->system); record->character->SetListener(_world->playerListener); record->node = node; record->generation = nodeGetGeneration(node); record->extent = extent; record->height = bounds.mMax.GetY() - bounds.mMin.GetY(); record->intent = vec3(0.0f, 0.0f, 0.0f); record->jumpSpeed = 0.0f; record->gravityScale = 1.0f; record->stepHeight = DEFAULT_STEP_HEIGHT * extent / DEFAULT_EXTENT; record->enabled = true; record->used = true; _indexSet(_world->playerOfNode, node, x); return true; } // A disabled player neither moves nor blocks: its inner body leaves the world, and any triggers // it stood in are left. bool playerSetEnabled(int32_t node, bool enabled) { PlayerRecordT *record = _findPlayer(node); JPH::BodyID inner; if (record == nullptr) { return false; } if (enabled == record->enabled) { return true; } inner = record->character->GetInnerBodyID(); if (enabled) { if (!inner.IsInvalid()) { _world->system->GetBodyInterface().AddBody(inner, JPH::EActivation::Activate); } } else { if (!inner.IsInvalid()) { _world->system->GetBodyInterface().RemoveBody(inner); } _playerInside(record, nullptr, 0); } record->enabled = enabled; return true; } bool playerSetGravityScale(int32_t node, float scale) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->gravityScale = scale; return true; } bool playerSetMass(int32_t node, float kilograms) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->character->SetMass(SDL_max(kilograms, MIN_MASS)); return true; } // Teleports; nothing in between is touched. bool playerSetPosition(int32_t node, Vec3T position) { PlayerRecordT *record = _findPlayer(node); Vec3T where; QuatT rotation; Vec3T scale; if (record == nullptr) { return false; } record->character->SetPosition(JPH::RVec3(position.x, position.y, position.z)); record->character->SetLinearVelocity(JPH::Vec3::sZero()); nodeGetWorldTransform(node, &where, &rotation, &scale); nodeSetWorldTransform(node, position, rotation); return true; } bool playerSetPush(int32_t node, float strength) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->character->SetMaxStrength(SDL_max(0.0f, strength)); return true; } bool playerSetSlope(int32_t node, float degrees) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->character->SetMaxSlopeAngle(JPH::DegreesToRadians(SDL_clamp(degrees, 0.0f, MAX_SLOPE_DEGREES))); return true; } bool playerSetStep(int32_t node, float height) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->stepHeight = SDL_max(0.0f, height); return true; } bool playerSetSwim(int32_t node, float sinkSpeed, float drag) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->sinkSpeed = SDL_max(0.0f, sinkSpeed); record->swimDrag = SDL_max(0.0f, drag); return true; } bool playerSetVelocity(int32_t node, Vec3T velocity) { PlayerRecordT *record = _findPlayer(node); if (record == nullptr) { return false; } record->character->SetLinearVelocity(JPH::Vec3(velocity.x, velocity.y, velocity.z)); record->intent = vec3(0.0f, 0.0f, 0.0f); return true; } // Physics takes over the skeleton from where the animation left it. bool ragdollActivate(int32_t node) { RagdollRecordT *record = _findRagdoll(node); JPH::BodyInterface *bodies; const int32_t *joints; int32_t jointCount; int32_t p; if (record == nullptr) { return false; } if (record->active) { return true; } bodies = &_world->system->GetBodyInterface(); animationStop(record->node, ANIMATION_ALL_LAYERS); sceneUpdateTransforms(); jointCount = nodeGetSkinJoints(record->skinned, &joints); record->filter = new JPH::GroupFilterTable((JPH::uint)record->partCount); // Bodies: a capsule along each bone, from the joint toward its children. for (p = 0; p < record->partCount; p++) { RagdollPartT *part = &record->parts[p]; Vec3T jointPosition; QuatT jointRotation; Vec3T jointScale; JPH::Vec3 origin; JPH::Vec3 toward = JPH::Vec3::sZero(); JPH::Vec3 direction; JPH::Quat rotation; JPH::RVec3 centre; float length; float radius; int32_t children = 0; int32_t c; nodeGetWorldTransform(part->joint, &jointPosition, &jointRotation, &jointScale); origin = JPH::Vec3(jointPosition.x, jointPosition.y, jointPosition.z); for (c = 0; c < nodeGetChildCount(part->joint); c++) { int32_t child = nodeGetChild(part->joint, c); Vec3T where; if (_ragdollPartOf(record, child) < 0) { // A leaf joint still gives the bone its length. bool isJoint = false; int32_t j; for (j = 0; j < jointCount; j++) { isJoint = isJoint || (joints[j] == child); } if (!isJoint) { continue; } } where = nodeGetWorldPosition(child); toward += JPH::Vec3(where.x, where.y, where.z); children++; } if (children > 0) { toward = toward / (float)children; } else { toward = origin + JPH::Vec3(0.0f, RAGDOLL_RADIUS_MIN * LEAF_BONE_RADII, 0.0f); } direction = toward - origin; length = SDL_max(direction.Length(), RAGDOLL_RADIUS_MIN * MIN_BONE_RADII); direction = (direction.LengthSq() > ZERO_LENGTH_SQ) ? direction.Normalized() : JPH::Vec3::sAxisY(); radius = (part->radius > 0.0f) ? part->radius : SDL_max(length * RAGDOLL_RADIUS_RATIO, RAGDOLL_RADIUS_MIN); rotation = JPH::Quat::sFromTo(JPH::Vec3::sAxisY(), direction); centre = JPH::RVec3(origin + direction * (length / 2.0f)); { JPH::CapsuleShapeSettings capsule(SDL_max(length / 2.0f - radius, MIN_DIMENSION), radius); JPH::BodyCreationSettings settings(capsule.Create().Get(), centre, rotation, JPH::EMotionType::Dynamic, LAYER_MOVING); settings.mUserData = (JPH::uint64)(uint32_t)part->joint; settings.mFriction = DEFAULT_FRICTION; settings.mRestitution = 0.0f; settings.mLinearDamping = RAGDOLL_LINEAR_DAMPING; settings.mAngularDamping = RAGDOLL_ANGULAR_DAMPING; settings.mCollisionGroup = JPH::CollisionGroup(record->filter, _world->ragdollGroups, (JPH::CollisionGroup::SubGroupID)p); part->body = bodies->CreateAndAddBody(settings, JPH::EActivation::Activate); } if (part->body.IsInvalid()) { _releaseRagdollBodies(record); return false; } part->offsetPosition = rotation.Conjugated() * (origin - JPH::Vec3(centre)); part->offsetRotation = rotation.Conjugated() * _fromQuat(jointRotation); part->scale = JPH::Vec3(jointScale.x, jointScale.y, jointScale.z); } _world->ragdollGroups++; // Joints: a swing-twist cone at each joint to the parent bone, parent-child collision off. for (p = 0; p < record->partCount; p++) { RagdollPartT *part = &record->parts[p]; RagdollPartT *parent; JPH::SwingTwistConstraintSettings settings; JPH::RVec3 pivot; JPH::Quat parentRotation; JPH::Quat rotation; JPH::Vec3 twist; JPH::Vec3 plane; if (part->parent < 0) { continue; } parent = &record->parts[part->parent]; record->filter->DisableCollision((JPH::CollisionGroup::SubGroupID)p, (JPH::CollisionGroup::SubGroupID)part->parent); bodies->GetPositionAndRotation(part->body, pivot, rotation); parentRotation = bodies->GetRotation(parent->body); pivot = pivot + rotation * part->offsetPosition; twist = rotation * JPH::Vec3::sAxisY(); plane = twist.GetNormalizedPerpendicular(); settings.mSpace = JPH::EConstraintSpace::WorldSpace; settings.mPosition1 = pivot; settings.mPosition2 = pivot; settings.mTwistAxis1 = twist; settings.mTwistAxis2 = twist; settings.mPlaneAxis1 = plane; settings.mPlaneAxis2 = plane; settings.mNormalHalfConeAngle = JPH::DegreesToRadians(part->swing); settings.mPlaneHalfConeAngle = JPH::DegreesToRadians(part->swing); settings.mTwistMinAngle = -JPH::DegreesToRadians(part->twist); settings.mTwistMaxAngle = JPH::DegreesToRadians(part->twist); settings.mSwingMotorSettings = JPH::MotorSettings(RAGDOLL_MOTOR_HZ, RAGDOLL_MOTOR_DAMPING); settings.mTwistMotorSettings = JPH::MotorSettings(RAGDOLL_MOTOR_HZ, RAGDOLL_MOTOR_DAMPING); { JPH::BodyID pair[2] = { parent->body, part->body }; JPH::BodyLockMultiWrite lock(_world->system->GetBodyLockInterface(), pair, 2); if ((lock.GetBody(0) == nullptr) || (lock.GetBody(1) == nullptr)) { continue; } part->constraint = settings.Create(*lock.GetBody(0), *lock.GetBody(1)); } _world->system->AddConstraint(part->constraint); part->rest = parentRotation.Conjugated() * rotation; } record->active = true; record->strengthChanged = true; return true; } // A shove on one bone, by its joint's name. bool ragdollApplyImpulse(int32_t node, const char *joint, Vec3T impulse) { RagdollRecordT *record = _findRagdoll(node); int32_t p; if ((record == nullptr) || !record->active) { return false; } for (p = 0; p < record->partCount; p++) { const char *name = nodeGetName(record->parts[p].joint); if ((name != nullptr) && (SDL_strcasecmp(name, joint) == 0)) { _world->system->GetBodyInterface().AddImpulse(record->parts[p].body, JPH::Vec3(impulse.x, impulse.y, impulse.z)); return true; } } return false; } // Animation may take over again; the joints keep the pose the ragdoll left them in. bool ragdollDeactivate(int32_t node) { RagdollRecordT *record = _findRagdoll(node); if (record == nullptr) { return false; } _releaseRagdollBodies(record); return true; } bool ragdollDelete(int32_t node) { RagdollRecordT *record = _findRagdoll(node); if (record == nullptr) { return false; } _releaseRagdoll(record); return true; } bool ragdollExists(int32_t node) { return _findRagdoll(node) != nullptr; } bool ragdollIsActive(int32_t node) { RagdollRecordT *record = _findRagdoll(node); return (record != nullptr) && record->active; } bool ragdollIsResting(int32_t node) { RagdollRecordT *record = _findRagdoll(node); int32_t p; if ((record == nullptr) || !record->active) { return false; } for (p = 0; p < record->partCount; p++) { if (_world->system->GetBodyInterface().IsActive(record->parts[p].body)) { return false; } } return true; } // Derives the parts from a model instance's skin: every joint with a child joint becomes a bone, // its parent bone the nearest ancestor joint that is one. Nothing is simulated until activated. bool ragdollNew(int32_t node) { RagdollRecordT *record = nullptr; const int32_t *joints; int32_t count; int32_t skinned; int32_t x; int32_t j; int32_t c; if ((_world == nullptr) || !nodeValid(node)) { return false; } skinned = _findSkinned(node); if (skinned < 0) { utilTrace("Physics: node %d has no skinned model under it for a ragdoll.", node); return false; } ragdollDelete(node); for (x = 0; x < _world->ragdollCount; x++) { if (!_world->ragdolls[x].used) { record = &_world->ragdolls[x]; break; } } if (record == nullptr) { utilTrace("Physics: no room for another ragdoll (%d already).", MAX_RAGDOLLS); return false; } _resetRagdoll(record); count = nodeGetSkinJoints(skinned, &joints); for (j = 0; (j < count) && (record->partCount < MAX_RAGDOLL_PARTS); j++) { bool hasChildJoint = false; for (c = 0; c < nodeGetChildCount(joints[j]); c++) { int32_t child = nodeGetChild(joints[j], c); for (x = 0; x < count; x++) { hasChildJoint = hasChildJoint || (joints[x] == child); } } if (!hasChildJoint) { continue; } record->parts[record->partCount].joint = joints[j]; record->parts[record->partCount].parent = -1; record->parts[record->partCount].radius = 0.0f; record->parts[record->partCount].swing = RAGDOLL_SWING_DEGREES; record->parts[record->partCount].twist = RAGDOLL_TWIST_DEGREES; record->partCount++; } if (record->partCount == 0) { utilTrace("Physics: the skin under node %d has no bones to make a ragdoll from.", node); return false; } // Parents: walk up the scene tree to the nearest joint that became a part. for (x = 0; x < record->partCount; x++) { int32_t up = nodeGetParent(record->parts[x].joint); while ((up >= 0) && (record->parts[x].parent < 0)) { record->parts[x].parent = _ragdollPartOf(record, up); up = nodeGetParent(up); } } record->node = node; record->generation = nodeGetGeneration(node); record->skinned = skinned; record->used = true; _indexSet(_world->ragdollOfNode, node, (int32_t)(record - _world->ragdolls)); return true; } // Tunes one bone before activation: capsule radius (0 = automatic), swing and twist limits. bool ragdollSetJoint(int32_t node, const char *joint, float radius, float swingDegrees, float twistDegrees) { RagdollRecordT *record = _findRagdoll(node); int32_t p; if (record == nullptr) { return false; } for (p = 0; p < record->partCount; p++) { const char *name = nodeGetName(record->parts[p].joint); if ((name != nullptr) && (SDL_strcasecmp(name, joint) == 0)) { record->parts[p].radius = SDL_max(0.0f, radius); record->parts[p].swing = SDL_clamp(swingDegrees, 0.0f, MAX_CONE_DEGREES); record->parts[p].twist = SDL_clamp(twistDegrees, 0.0f, MAX_CONE_DEGREES); return true; } } return false; } // 0 goes limp; more drives every joint back toward the pose it was activated in. bool ragdollSetStrength(int32_t node, float strength) { RagdollRecordT *record = _findRagdoll(node); if (record == nullptr) { return false; } record->strength = SDL_max(0.0f, strength); record->strengthChanged = true; return true; } bool softDelete(int32_t node) { SoftRecordT *record = _findSoft(node); if (record == nullptr) { return false; } _releaseSoft(record); return true; } bool softExists(int32_t node) { return _findSoft(node) != nullptr; } // The node's mesh becomes cloth or a pressure body: its vertices, welded by position, are the // particles, in world space where the node has put them. Welding hashes each vertex's SOFT_WELD // cell, so vertices split for their UVs or normals (the same position exactly) become one particle. bool softNew(int32_t node, SoftKindE kind) { SoftRecordT *record = nullptr; const float *positions; const uint32_t *indices; int32_t vertexCount; int32_t indexCount; int32_t mesh; Vec3T position; QuatT rotation; Vec3T scale; int32_t x; std::unordered_map welded; if ((_world == nullptr) || !nodeValid(node) || (kind == SOFT_ROPE)) { return false; } // Any soft body already here goes first: a rope's tube mesh goes with it, so the geometry is // read only afterwards. softDelete(node); mesh = nodeGetMesh(node); if (!meshGetGeometry(mesh, &positions, &vertexCount, &indices, &indexCount) || (vertexCount < 3)) { utilTrace("Physics: node %d needs a mesh to become a soft body.", node); return false; } for (x = 0; x < _world->softCount; x++) { if (!_world->softs[x].used) { record = &_world->softs[x]; break; } } if (record == nullptr) { utilTrace("Physics: no room for another soft body (%d already).", MAX_SOFT); return false; } _resetSoft(record); sceneUpdateTransforms(); nodeGetWorldTransform(node, &position, &rotation, &scale); record->meshToSoft = (int32_t *)SDL_calloc((size_t)vertexCount, sizeof(int32_t)); record->positions = (float *)SDL_calloc((size_t)vertexCount * 3, sizeof(float)); record->meshPositions = (float *)SDL_calloc((size_t)vertexCount * 3, sizeof(float)); if ((record->meshToSoft == nullptr) || (record->positions == nullptr) || (record->meshPositions == nullptr)) { utilDie("Out of memory for a soft body."); } record->meshVertexCount = vertexCount; welded.reserve((size_t)vertexCount); for (x = 0; x < vertexCount; x++) { Vec3T local = vec3(positions[x * 3] * scale.x, positions[x * 3 + 1] * scale.y, positions[x * 3 + 2] * scale.z); Vec3T world = vec3Add(position, quatRotate(rotation, local)); WeldKeyT key = { (int32_t)floorf(world.x / SOFT_WELD), (int32_t)floorf(world.y / SOFT_WELD), (int32_t)floorf(world.z / SOFT_WELD) }; std::unordered_map::iterator it = welded.find(key); int32_t found; if (it != welded.end()) { found = it->second; } else { found = record->count++; record->positions[found * 3] = world.x; record->positions[found * 3 + 1] = world.y; record->positions[found * 3 + 2] = world.z; welded[key] = found; } record->meshToSoft[x] = found; } record->node = node; record->generation = nodeGetGeneration(node); record->kind = kind; record->mesh = mesh; record->used = true; _indexSet(_world->softOfNode, node, (int32_t)(record - _world->softs)); if (!_buildSoft(record)) { _releaseSoft(record); return false; } return true; } // A rope from the node's position to a world point: particles along it, a tube mesh round them // that becomes the node's mesh (keeping its material). bool softNewRope(int32_t node, Vec3T end, int32_t segments, float radius) { SoftRecordT *record = nullptr; SceneVertexT *vertices; uint32_t *indices; Vec3T *rings; Vec3T start; Vec3T position; QuatT rotation; Vec3T scale; QuatT inverse; int32_t indexCount; int32_t count; int32_t mesh; int32_t x; int32_t side; if ((_world == nullptr) || !nodeValid(node) || (segments < 1) || (radius <= 0.0f)) { return false; } softDelete(node); for (x = 0; x < _world->softCount; x++) { if (!_world->softs[x].used) { record = &_world->softs[x]; break; } } if (record == nullptr) { utilTrace("Physics: no room for another soft body (%d already).", MAX_SOFT); return false; } _resetSoft(record); sceneUpdateTransforms(); nodeGetWorldTransform(node, &position, &rotation, &scale); start = position; count = segments + 1; indexCount = segments * ROPE_SIDES * ROPE_INDICES_PER_QUAD; record->count = count; record->meshVertexCount = count * ROPE_SIDES; record->positions = (float *)SDL_calloc((size_t)count * 3, sizeof(float)); record->meshPositions = (float *)SDL_calloc((size_t)record->meshVertexCount * 3, sizeof(float)); record->meshToSoft = (int32_t *)SDL_calloc((size_t)record->meshVertexCount, sizeof(int32_t)); vertices = (SceneVertexT *)SDL_calloc((size_t)record->meshVertexCount, sizeof(SceneVertexT)); indices = (uint32_t *)SDL_calloc((size_t)indexCount, sizeof(uint32_t)); rings = (Vec3T *)SDL_calloc((size_t)record->meshVertexCount, sizeof(Vec3T)); if ((record->positions == nullptr) || (record->meshPositions == nullptr) || (record->meshToSoft == nullptr) || (vertices == nullptr) || (indices == nullptr) || (rings == nullptr)) { utilDie("Out of memory for a rope."); } for (x = 0; x < count; x++) { float t = (float)x / (float)segments; record->positions[x * 3] = start.x + (end.x - start.x) * t; record->positions[x * 3 + 1] = start.y + (end.y - start.y) * t; record->positions[x * 3 + 2] = start.z + (end.z - start.z) * t; } record->ropeRadius = radius; record->kind = SOFT_ROPE; // The tube, in the node's frame, with the rope's length running along V. _ropeMesh(record, rings); inverse = quatInverse(rotation); for (x = 0; x < count; x++) { for (side = 0; side < ROPE_SIDES; side++) { int32_t v = x * ROPE_SIDES + side; Vec3T local = quatRotate(inverse, vec3Subtract(rings[v], position)); vertices[v].position[0] = local.x / SDL_max(scale.x, MIN_SCALE); vertices[v].position[1] = local.y / SDL_max(scale.y, MIN_SCALE); vertices[v].position[2] = local.z / SDL_max(scale.z, MIN_SCALE); vertices[v].uv[0] = (float)side / (float)ROPE_SIDES; vertices[v].uv[1] = (float)x / (float)segments; vertices[v].weights[0] = 1.0f; record->meshToSoft[v] = x; } } for (x = 0; x < segments; x++) { for (side = 0; side < ROPE_SIDES; side++) { uint32_t a = (uint32_t)(x * ROPE_SIDES + side); uint32_t b = (uint32_t)(x * ROPE_SIDES + (side + 1) % ROPE_SIDES); uint32_t c = a + ROPE_SIDES; uint32_t d = b + ROPE_SIDES; uint32_t *tri = &indices[(x * ROPE_SIDES + side) * ROPE_INDICES_PER_QUAD]; tri[0] = a; tri[1] = c; tri[2] = b; tri[3] = b; tri[4] = c; tri[5] = d; } } sceneComputeNormals(vertices, record->meshVertexCount, indices, indexCount); mesh = meshNewVertices(vertices, record->meshVertexCount, indices, indexCount, false); SDL_free(vertices); SDL_free(indices); SDL_free(rings); if (mesh == NO_HANDLE) { _releaseSoft(record); return false; } nodeSetMesh(node, mesh, nodeGetMaterial(node)); record->node = node; record->generation = nodeGetGeneration(node); record->mesh = mesh; record->used = true; _indexSet(_world->softOfNode, node, (int32_t)(record - _world->softs)); if (!_buildSoft(record)) { _releaseSoft(record); return false; } return true; } // Holds the particle nearest a world point where it is, or to a node from now on. bool softPin(int32_t node, Vec3T point, int32_t follow) { SoftRecordT *record = _findSoft(node); int32_t vertex; int32_t x; if ((record == nullptr) || (record->pinCount >= MAX_SOFT_PINS)) { return false; } vertex = _nearestSoftVertex(record, point); if (vertex < 0) { return false; } for (x = 0; x < record->pinCount; x++) { if (record->pins[x].vertex == vertex) { record->pins[x].follow = follow; return true; } } record->pins[record->pinCount].vertex = vertex; record->pins[record->pinCount].follow = follow; record->pinCount++; if (!record->body.IsInvalid()) { JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->body); if (lock.Succeeded()) { JPH::SoftBodyMotionProperties *motion = static_cast(lock.GetBody().GetMotionProperties()); motion->GetVertex((JPH::uint)vertex).mInvMass = 0.0f; motion->GetVertex((JPH::uint)vertex).mVelocity = JPH::Vec3::sZero(); } } return true; } // Damping is a live setting of the body; nothing is rebuilt. bool softSetDamping(int32_t node, float damping) { SoftRecordT *record = _findSoft(node); if (record == nullptr) { return false; } record->damping = SDL_max(0.0f, damping); if (!record->body.IsInvalid()) { { JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->body); if (lock.Succeeded()) { lock.GetBody().GetMotionProperties()->SetLinearDamping(record->damping); } } // Outside the lock: activating takes locks of its own. _world->system->GetBodyInterface().ActivateBody(record->body); } return true; } // Mass is shared out over the particles in place; nothing is rebuilt. bool softSetMass(int32_t node, float kilograms) { SoftRecordT *record = _findSoft(node); if (record == nullptr) { return false; } record->mass = SDL_max(MIN_MASS, kilograms); _softSetMasses(record); return true; } bool softSetPressure(int32_t node, float pressure) { SoftRecordT *record = _findSoft(node); if (record == nullptr) { return false; } record->pressure = SDL_max(0.0f, pressure); if (!record->body.IsInvalid()) { { JPH::BodyLockWrite lock(_world->system->GetBodyLockInterface(), record->body); if (lock.Succeeded()) { static_cast(lock.GetBody().GetMotionProperties())->SetPressure(record->pressure); } } // Outside the lock: activating takes locks of its own. _world->system->GetBodyInterface().ActivateBody(record->body); } return true; } // Stretch and bend stiffness, 0 to 1; the constraints are remade around the current shape. bool softSetStiffness(int32_t node, float stretch, float bend) { SoftRecordT *record = _findSoft(node); if (record == nullptr) { return false; } record->stretch = SDL_clamp(stretch, 0.0f, 1.0f); record->bend = SDL_clamp(bend, 0.0f, 1.0f); if (!_buildSoft(record)) { _releaseSoft(record); return false; } return true; } // Lets the particle nearest a world point go, giving it its share of the mass back. bool softUnpin(int32_t node, Vec3T point) { SoftRecordT *record = _findSoft(node); int32_t vertex; int32_t x; if (record == nullptr) { return false; } vertex = _nearestSoftVertex(record, point); for (x = 0; x < record->pinCount; x++) { if (record->pins[x].vertex == vertex) { record->pins[x] = record->pins[record->pinCount - 1]; record->pinCount--; _softSetMasses(record); return true; } } return false; } // A wheel at the wheel node's position relative to the chassis; returns its index. int32_t vehicleAddWheel(int32_t node, int32_t wheelNode, float radius, float width, float suspension) { VehicleRecordT *record = _findVehicle(node); WheelRecordT *wheel; Vec3T chassisPosition; QuatT chassisRotation; Vec3T chassisScale; if ((record == nullptr) || !nodeValid(wheelNode) || (record->wheelCount >= MAX_WHEELS)) { return -1; } // The attachment point is the wheel node's place in the chassis' frame right now (the body's // frame is unscaled: the chassis' scale is baked into its shape); the engine poses the node // with suspension travel from here on, so a later rebuild must not read it back. sceneUpdateTransforms(); nodeGetWorldTransform(node, &chassisPosition, &chassisRotation, &chassisScale); wheel = &record->wheels[record->wheelCount]; wheel->node = wheelNode; wheel->generation = nodeGetGeneration(wheelNode); wheel->rest = quatRotate(quatInverse(chassisRotation), vec3Subtract(nodeGetWorldPosition(wheelNode), chassisPosition)); wheel->radius = SDL_max(radius, MIN_DIMENSION); wheel->width = SDL_max(width, MIN_DIMENSION); wheel->suspension = SDL_max(suspension, MIN_DIMENSION); wheel->steered = false; wheel->driven = true; wheel->steeredSet = false; record->dirty = true; return record->wheelCount++; } bool vehicleDelete(int32_t node) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } _releaseVehicle(record); return true; } bool vehicleDrive(int32_t node, float forward, float right, float brake, float handBrake) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->inputForward = SDL_clamp(forward, -1.0f, 1.0f); record->inputRight = SDL_clamp(right, -1.0f, 1.0f); record->inputBrake = SDL_clamp(brake, 0.0f, 1.0f); record->inputHandBrake = SDL_clamp(handBrake, 0.0f, 1.0f); return true; } bool vehicleExists(int32_t node) { return _findVehicle(node) != nullptr; } int32_t vehicleGetGear(int32_t node) { VehicleRecordT *record = _findVehicle(node); if ((record == nullptr) || (record->constraint == nullptr)) { return 0; } if (record->kind == VEHICLE_TANK) { return static_cast(record->constraint->GetController())->GetTransmission().GetCurrentGear(); } return static_cast(record->constraint->GetController())->GetTransmission().GetCurrentGear(); } float vehicleGetRpm(int32_t node) { VehicleRecordT *record = _findVehicle(node); if ((record == nullptr) || (record->constraint == nullptr)) { return 0.0f; } if (record->kind == VEHICLE_TANK) { return static_cast(record->constraint->GetController())->GetEngine().GetCurrentRPM(); } return static_cast(record->constraint->GetController())->GetEngine().GetCurrentRPM(); } // Metres a second along the chassis' nose, negative in reverse. float vehicleGetSpeed(int32_t node) { VehicleRecordT *record = _findVehicle(node); BodyRecordT *body; JPH::Vec3 velocity; JPH::Quat rotation; if ((record == nullptr) || ((body = _find(record->node)) == nullptr)) { return 0.0f; } velocity = _world->system->GetBodyInterface().GetLinearVelocity(body->id); rotation = _world->system->GetBodyInterface().GetRotation(body->id); return velocity.Dot(rotation * JPH::Vec3(0.0f, 0.0f, -1.0f)); } // Longitudinal slip of a wheel, 0 gripping to about 1 spinning or locked. float vehicleGetWheelSlip(int32_t node, int32_t index) { VehicleRecordT *record = _findVehicle(node); if ((record == nullptr) || (record->constraint == nullptr) || (index < 0) || (index >= record->wheelCount) || (record->kind == VEHICLE_TANK)) { return 0.0f; } return SDL_min(fabsf(static_cast(record->constraint->GetWheel((JPH::uint)index))->mLongitudinalSlip), 1.0f); } bool vehicleIsWheelOnGround(int32_t node, int32_t index) { VehicleRecordT *record = _findVehicle(node); if ((record == nullptr) || (record->constraint == nullptr) || (index < 0) || (index >= record->wheelCount)) { return false; } return record->constraint->GetWheel((JPH::uint)index)->HasContact(); } // A vehicle on the node, whose dynamic body is the chassis; add wheels before driving it. bool vehicleNew(int32_t node, VehicleKindE kind) { VehicleRecordT *record = nullptr; BodyRecordT *body = _find(node); int32_t x; if ((_world == nullptr) || (body == nullptr) || (body->type != BODY_DYNAMIC)) { utilTrace("Physics: a vehicle needs a dynamic body on node %d first.", node); return false; } if (_world->planar && (kind != VEHICLE_BOAT)) { utilTrace("Physics: vehicles need a 3D world; a 2D car is a body with hinged wheels."); return false; } vehicleDelete(node); for (x = 0; x < _world->vehicleCount; x++) { if (!_world->vehicles[x].used) { record = &_world->vehicles[x]; break; } } if (record == nullptr) { utilTrace("Physics: no room for another vehicle (%d already).", MAX_VEHICLES); return false; } _resetVehicle(record); record->node = node; record->generation = nodeGetGeneration(node); record->kind = kind; record->dirty = (kind != VEHICLE_BOAT); record->used = true; _indexSet(_world->vehicleOfNode, node, x); return true; } bool vehicleSetAntiRoll(int32_t node, float stiffness) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->antiRoll = SDL_max(0.0f, stiffness); record->dirty = true; return true; } bool vehicleSetBrakes(int32_t node, float brake, float handBrake) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->brakeTorque = SDL_max(0.0f, brake); record->handBrakeTorque = SDL_max(0.0f, handBrake); record->dirty = true; return true; } bool vehicleSetEngine(int32_t node, float maxTorque, float maxRpm, float minRpm) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->maxTorque = SDL_max(MIN_ENGINE_TORQUE, maxTorque); record->maxRpm = SDL_max(MIN_ENGINE_MAX_RPM, maxRpm); record->minRpm = SDL_clamp(minRpm, MIN_ENGINE_RPM, record->maxRpm); record->dirty = true; return true; } bool vehicleSetGears(int32_t node, const float *ratios, int32_t count, float reverse, bool automatic) { VehicleRecordT *record = _findVehicle(node); int32_t x; if ((record == nullptr) || (count < 1) || (count > VEHICLE_MAX_GEARS)) { return false; } for (x = 0; x < count; x++) { record->gears[x] = ratios[x]; } record->gearCount = count; record->reverseGear = (reverse > 0.0f) ? -reverse : reverse; record->automatic = automatic; record->dirty = true; return true; } bool vehicleSetRudder(int32_t node, float maxTorque) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->rudder = SDL_max(0.0f, maxTorque); return true; } bool vehicleSetSteering(int32_t node, float maxDegrees) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->maxSteer = SDL_clamp(maxDegrees, 0.0f, MAX_STEER_DEGREES); record->dirty = true; return true; } bool vehicleSetSuspension(int32_t node, float frequency, float damping) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->suspensionHz = SDL_max(MIN_SUSPENSION_HZ, frequency); record->suspensionDamping = SDL_max(0.0f, damping); record->dirty = true; return true; } bool vehicleSetThrust(int32_t node, float maxForce, Vec3T point) { VehicleRecordT *record = _findVehicle(node); if (record == nullptr) { return false; } record->thrust = SDL_max(0.0f, maxForce); record->thrustPoint = point; return true; } bool vehicleSetWheel(int32_t node, int32_t index, bool steered, bool driven) { VehicleRecordT *record = _findVehicle(node); if ((record == nullptr) || (index < 0) || (index >= record->wheelCount)) { return false; } record->wheels[index].steered = steered; record->wheels[index].driven = driven; record->wheels[index].steeredSet = true; record->dirty = true; return true; }