Public_Skunkworks/singe
Public_Skunkworks/singe
1
0
Fork 0
Code Issues 7 Pull requests Projects Releases Packages Wiki Activity Actions
singe/src/physicsJolt.cpp

4134 lines
141 KiB
C++
Raw Blame History

/*
*
* Singe 3
* Copyright (C) 2006-2026 Scott Duensing <scott@kangaroopunch.com>
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 3
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
* 02110-1301, USA.
*
*/
// Jolt Physics behind the C interface in physics.h: the only C++ in Singe. Nothing of Jolt's
// crosses the header; the engine sees plain C functions and node handles.
#include <SDL3/SDL.h>
#include <Jolt/Jolt.h>
#include <Jolt/RegisterTypes.h>
#include <Jolt/Core/Factory.h>
#include <Jolt/Core/TempAllocator.h>
#include <Jolt/Core/JobSystemThreadPool.h>
#include <Jolt/Physics/PhysicsSettings.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/Physics/Body/BodyCreationSettings.h>
#include <Jolt/Physics/Body/BodyLock.h>
#include <Jolt/Physics/Body/BodyLockMulti.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
#include <Jolt/Physics/Collision/ObjectLayer.h>
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
#include <Jolt/Physics/Collision/Shape/MeshShape.h>
#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
#include <Jolt/Physics/Constraints/HingeConstraint.h>
#include <Jolt/Physics/Constraints/PointConstraint.h>
#include <Jolt/Physics/Constraints/SliderConstraint.h>
#include <Jolt/Physics/Collision/CastResult.h>
#include <Jolt/Physics/Collision/RayCast.h>
#include <Jolt/Physics/Collision/ContactListener.h>
#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
#include <Jolt/Physics/Character/CharacterVirtual.h>
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
#include <Jolt/Physics/Vehicle/TrackedVehicleController.h>
#include <Jolt/Physics/Vehicle/MotorcycleController.h>
#include <Jolt/Physics/Vehicle/VehicleCollisionTester.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
#include <Jolt/Physics/Collision/TransformedShape.h>
#include <Jolt/Physics/Collision/GroupFilterTable.h>
#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRendererSimple.h>
#endif
#include <mutex>
#include <unordered_map>
#include <vector>
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<JPH::CharacterVirtual> 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<JPH::Constraint> 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<JPH::GroupFilterTable> 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<JPH::SoftBodySharedSettings> 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<JPH::VehicleConstraint> constraint;
JPH::Ref<JPH::VehicleCollisionTester> 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<PhysicsEventT> events;
std::unordered_map<uint64_t, int32_t> 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); // Drops the overlap counts a node is party to
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<JPH::Constraint> 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<JointRecordT> 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<int32_t> bodyOfNode; // Node handle to slot in each table, or NO_HANDLE
std::vector<int32_t> playerOfNode;
std::vector<int32_t> vehicleOfNode;
std::vector<int32_t> ragdollOfNode;
std::vector<int32_t> 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<JPH::Shape> _buildHeightField(int32_t mesh, Vec3T scale);
JPH::RefConst<JPH::Shape> _buildMeshShape(int32_t node, ShapeTypeE shape, Vec3T position, QuatT rotation);
JPH::RefConst<JPH::Shape> _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<JPH::Vec3> &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);
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<int32_t> &index, int32_t node);
void _indexSet(std::vector<int32_t> &index, int32_t node, int32_t slot);
int32_t _nearestSoftVertex(const SoftRecordT *record, Vec3T point);
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 _releasePlayer(PlayerRecordT *record);
void _releaseRagdoll(RagdollRecordT *record);
void _releaseRagdollBodies(RagdollRecordT *record);
void _releaseSoft(SoftRecordT *record);
void _releaseSoftBody(SoftRecordT *record);
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 (_findPlayer(node) == nullptr) {
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 (_findPlayer(node) == nullptr) {
overlap((int32_t)(uint32_t)trigger->GetUserData(), node, false);
}
}
void ContactListenerT::forget(int32_t node) {
std::lock_guard<std::mutex> guard(lock);
std::unordered_map<uint64_t, int32_t>::iterator it = overlaps.begin();
while (it != overlaps.end()) {
if (((int32_t)(uint32_t)(it->first >> 32) == node) || ((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<std::mutex> guard(lock);
if (entered) {
if (++overlaps[key] != 1) {
return;
}
} else {
std::unordered_map<uint64_t, int32_t>::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<std::mutex> 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<std::mutex> 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;
}
if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
_release(record);
continue;
}
nodeGetWorldTransform(record->node, &position, &rotation, &scale);
bodies.MoveKinematic(record->id, JPH::RVec3(position.x, position.y, position.z), _fromQuat(rotation), dt);
}
while ((_world->accumulator >= STEP_SECONDS) && (steps < MAX_STEPS_PER_FRAME)) {
_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;
}
if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
_release(record);
continue;
}
bodies.GetPositionAndRotation(record->id, where, how);
nodeSetWorldTransform(record->node, vec3((float)where.GetX(), (float)where.GetY(), (float)where.GetZ()), _toQuat(how));
}
}
}
// 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<JPH::Shape> _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<JPH::Shape> _buildMeshShape(int32_t node, ShapeTypeE shape, Vec3T position, QuatT rotation) {
JPH::Array<JPH::Vec3> points;
JPH::VertexList vertices;
JPH::IndexedTriangleList triangles;
Mat4T bodyWorld = mat4Compose(position, rotation, vec3(1.0f, 1.0f, 1.0f));
Mat4T toBody;
if (!mat4Invert(bodyWorld, &toBody)) {
return nullptr;
}
_collectGeometry(node, &toBody, 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<JPH::Vec3> &points, JPH::IndexedTriangleList &triangles, JPH::VertexList &vertices) {
const float *positions;
const uint32_t *indices;
int32_t vertexCount;
int32_t indexCount;
int32_t mesh = nodeGetMesh(node);
int32_t x;
if ((mesh != NO_HANDLE) && meshGetGeometry(mesh, &positions, &vertexCount, &indices, &indexCount)) {
Vec3T position;
QuatT rotation;
Vec3T scale;
Mat4T local;
uint32_t base = (uint32_t)vertices.size();
nodeGetWorldTransform(node, &position, &rotation, &scale);
local = mat4Multiply(*toBody, mat4Compose(position, rotation, scale));
for (x = 0; x < vertexCount; x++) {
Vec3T v = mat4TransformPoint(local, vec3(positions[x * 3], positions[x * 3 + 1], positions[x * 3 + 2]));
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. A record whose node was deleted (or reused) is
// released on the way.
BodyRecordT *_find(int32_t node) {
BodyRecordT *record;
int32_t slot;
if (_world == nullptr) {
return nullptr;
}
slot = _indexGet(_world->bodyOfNode, node);
if (slot == NO_HANDLE) {
return nullptr;
}
record = &_world->bodies[slot];
if (!nodeValid(node) || (nodeGetGeneration(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<int32_t> &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<int32_t> &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;
}
// 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);
_indexSet(_world->bodyOfNode, record->node, NO_HANDLE);
_destroyBody(record->id);
memset(record, 0, sizeof(*record));
}
// 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<JPH::Shape> _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<JPH::Shape> 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) {
int32_t slot = (_world != nullptr) ? _indexGet(_world->playerOfNode, node) : NO_HANDLE;
return (slot == NO_HANDLE) ? nullptr : &_world->players[slot];
}
// 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 = 0;
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;
}
if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
_releasePlayer(record);
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) {
int32_t slot = (_world != nullptr) ? _indexGet(_world->softOfNode, node) : NO_HANDLE;
return (slot == NO_HANDLE) ? nullptr : &_world->softs[slot];
}
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 = 0;
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<JPH::SoftBodySharedSettings::VertexAttributes> 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<JPH::SoftBodyMotionProperties *>(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<JPH::SoftBodyMotionProperties *>(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 (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation) || (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<const JPH::SoftBodyMotionProperties *>(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) {
int32_t slot = (_world != nullptr) ? _indexGet(_world->ragdollOfNode, node) : NO_HANDLE;
return (slot == NO_HANDLE) ? nullptr : &_world->ragdolls[slot];
}
// 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 = 0;
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<JPH::SwingTwistConstraint *>(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;
}
if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
_releaseRagdoll(record);
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) {
int32_t slot = (_world != nullptr) ? _indexGet(_world->vehicleOfNode, node) : NO_HANDLE;
return (slot == NO_HANDLE) ? nullptr : &_world->vehicles[slot];
}
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 = 0;
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<JPH::CollideShapeBodyCollector> 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<JPH::MotorcycleControllerSettings *>(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<JPH::TrackedVehicleController *>(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<JPH::WheeledVehicleController *>(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 (!nodeValid(record->wheels[w].node) || (nodeGetGeneration(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<JPH::CollideShapeBodyCollector> 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;
}
if (!nodeValid(record->node) || (nodeGetGeneration(record->node) != record->generation)) {
_releasePlayer(record);
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);
}
}
// ===== Bodies =====
// A force for this step, at the centre of mass or a world point.
bool bodyApplyForce(int32_t node, Vec3T force, const Vec3T *at) {
BodyRecordT *record = _find(node);
if ((record == nullptr) || (record->type != BODY_DYNAMIC)) {
return false;
}
if (at != nullptr) {
_world->system->GetBodyInterface().AddForce(record->id, _fromVec3(force), JPH::RVec3(at->x, at->y, at->z));
} else {
_world->system->GetBodyInterface().AddForce(record->id, _fromVec3(force));
}
return true;
}
// An instant change of momentum, at the centre of mass or a world point.
bool bodyApplyImpulse(int32_t node, Vec3T impulse, const Vec3T *at) {
BodyRecordT *record = _find(node);
if ((record == nullptr) || (record->type != BODY_DYNAMIC)) {
return false;
}
if (at != nullptr) {
_world->system->GetBodyInterface().AddImpulse(record->id, _fromVec3(impulse), JPH::RVec3(at->x, at->y, at->z));
} else {
_world->system->GetBodyInterface().AddImpulse(record->id, _fromVec3(impulse));
}
return true;
}
bool bodyDelete(int32_t node) {
BodyRecordT *record = _find(node);
if (record == nullptr) {
return false;
}
_release(record);
return true;
}
bool bodyExists(int32_t node) {
return _find(node) != nullptr;
}
Vec3T bodyGetAngularVelocity(int32_t node) {
BodyRecordT *record = _find(node);
if (record == nullptr) {
return vec3(0.0f, 0.0f, 0.0f);
}
return _toVec3(_world->system->GetBodyInterface().GetAngularVelocity(record->id));
}
Vec3T bodyGetVelocity(int32_t node) {
BodyRecordT *record = _find(node);
if (record == nullptr) {
return vec3(0.0f, 0.0f, 0.0f);
}
return _toVec3(_world->system->GetBodyInterface().GetLinearVelocity(record->id));
}
// Asleep: a dynamic body that has come to rest (static and disabled bodies count as resting).
bool bodyIsResting(int32_t node) {
BodyRecordT *record = _find(node);
if (record == nullptr) {
return false;
}
return !record->enabled || !_world->system->GetBodyInterface().IsActive(record->id);
}
// Gives the node a body with a shape sized by a, b, c (see ShapeTypeE) and the node's world scale,
// placed where the node is now. One body per node; a second call replaces the first.
bool bodyNew(int32_t node, BodyTypeE type, ShapeTypeE shape, float a, float b, float c) {
BodyRecordT *record;
JPH::RefConst<JPH::Shape> joltShape;
Vec3T position;
QuatT rotation;
Vec3T scale;
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;
}
// 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);
}
record->trigger = trigger;
return true;
}
bool bodySetVelocity(int32_t node, Vec3T velocity) {
BodyRecordT *record = _find(node);
if ((record == nullptr) || (record->type == BODY_STATIC)) {
return false;
}
_world->system->GetBodyInterface().SetLinearVelocity(record->id, _fromVec3(velocity));
return true;
}
// ===== Joints =====
bool jointDelete(int32_t joint) {
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<JPH::Constraint> 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<JPH::HingeConstraint *>(_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<JPH::SliderConstraint *>(_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;
}
// ===== World =====
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<std::mutex> guard(_world->contacts->lock);
std::vector<PhysicsEventT> &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;
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: 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;
}
now = SDL_GetTicksNS();
if (advance && _world->enabled && (_world->lastTick != 0)) {
_world->accumulator += (double)(now - _world->lastTick) / 1e9;
}
_world->lastTick = now;
if (_world->accumulator > STEP_SECONDS * MAX_STEPS_PER_FRAME) {
_world->accumulator = STEP_SECONDS * MAX_STEPS_PER_FRAME;
}
if (_world->accumulator >= STEP_SECONDS) {
_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);
}
// 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<JPH::Shape> joltShape;
JPH::RefConst<JPH::Shape> 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 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;
}
// 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<JPH::TrackedVehicleController *>(record->constraint->GetController())->GetTransmission().GetCurrentGear();
}
return static_cast<JPH::WheeledVehicleController *>(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<JPH::TrackedVehicleController *>(record->constraint->GetController())->GetEngine().GetCurrentRPM();
}
return static_cast<JPH::WheeledVehicleController *>(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<const JPH::WheelWV *>(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 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 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;
}
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;
}
// 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 playerIsSwimming(int32_t node) {
PlayerRecordT *record = _findPlayer(node);
return (record != nullptr) && record->swimming;
}
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 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 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;
}
// Physics takes over the skeleton from where the animation left it.
bool ragdollActivate(int32_t node) {
RagdollRecordT *record = _findRagdoll(node);
JPH::BodyInterface &bodies = _world->system->GetBodyInterface();
const int32_t *joints;
int32_t jointCount;
int32_t p;
if (record == nullptr) {
return false;
}
if (record->active) {
return true;
}
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<WeldKeyT, int32_t, WeldHashT> welded;
if ((_world == nullptr) || !nodeValid(node) || (kind == SOFT_ROPE)) {
return false;
}
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;
}
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);
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<WeldKeyT, int32_t, WeldHashT>::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<JPH::SoftBodyMotionProperties *>(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<JPH::SoftBodyMotionProperties *>(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;
}
Reference in a new issue View git blame Copy permalink