retrieval of collision events
This commit is contained in:
parent
52b972167a
commit
3a2dfb0833
@ -75,166 +75,33 @@ end
|
||||
local useDistance = 6
|
||||
local pullDistance = useDistance * 4
|
||||
|
||||
-- on tick
|
||||
ent:bind( "tick", function(self)
|
||||
local inControl = scene:globalFindByName("Gui: Menu"):uid() == 0
|
||||
|
||||
local keyE = inputs.key("E") or inputs.key("R_Y")
|
||||
local keyF = inputs.key("F")
|
||||
local mouse1 = inputs.key("Mouse1") or inputs.key("L_TRIGGER");
|
||||
local mouse2 = inputs.key("Mouse2") or inputs.key("R_TRIGGER");
|
||||
local mouse3 = inputs.key("Mouse3");
|
||||
local wheel = inputs.analog("MouseWheel")
|
||||
|
||||
if not inControl then
|
||||
keyE = false
|
||||
keyF = false
|
||||
mouse1 = false
|
||||
mouse2 = false
|
||||
mouse3 = false
|
||||
wheel = 0
|
||||
end
|
||||
|
||||
-- eye transform
|
||||
local flattenedTransform = nil
|
||||
|
||||
if fixedCamera then
|
||||
flattenedTransform = transform:flatten()
|
||||
else
|
||||
flattenedTransform = cameraTransform:flatten()
|
||||
end
|
||||
|
||||
-- toggle flashlight
|
||||
local function tickFlashlight( transform, inputs )
|
||||
-- update light position
|
||||
if light.enabled then
|
||||
local center = flattenedTransform.position
|
||||
local direction = flattenedTransform.forward * 8
|
||||
|
||||
local center = transform.position
|
||||
local direction = transform.forward * 8
|
||||
local offset = 0.25
|
||||
local _, depth = physicsBody:rayCast( center, direction )
|
||||
if depth >= 0.5 then
|
||||
depth = 0.5
|
||||
elseif depth < 0 then
|
||||
depth = 0.5
|
||||
end
|
||||
local _, depth = physicsBody:rayCast(center, direction)
|
||||
depth = math.clamp(depth, 0, 0.5)
|
||||
light.transform.position = center + direction * (depth - offset)
|
||||
end
|
||||
|
||||
if timers.flashlight:elapsed() > 0.5 and keyF then
|
||||
-- toggle
|
||||
if timers.flashlight:elapsed() > 0.5 and inputs["F"] then
|
||||
timers.flashlight:reset()
|
||||
|
||||
if light.metadata.power ~= light.power then
|
||||
light.metadata.power = light.power
|
||||
light.enabled = true
|
||||
else
|
||||
light.metadata.power = 0
|
||||
light.enabled = false
|
||||
end
|
||||
|
||||
light.enabled = (light.metadata.power ~= light.power)
|
||||
light.metadata.power = light.enabled and light.power or 0
|
||||
playSound("flashlight")
|
||||
end
|
||||
end
|
||||
|
||||
-- fire use ray
|
||||
if timers.use:elapsed() > 0.5 and keyE and inControl then
|
||||
timers.use:reset()
|
||||
|
||||
local center = flattenedTransform.position
|
||||
local direction = flattenedTransform.forward * useDistance
|
||||
|
||||
local prop, depth = physicsBody:rayCast( center, direction )
|
||||
local payload = {
|
||||
user = ent:uid(),
|
||||
uid = prop and prop:uid() or 0,
|
||||
depth = depth,
|
||||
}
|
||||
if prop then
|
||||
prop:lazyCallHook("entity:Use.%UID%", payload)
|
||||
end
|
||||
ent:lazyCallHook("entity:Use.%UID%", payload)
|
||||
end
|
||||
|
||||
-- update HOLP
|
||||
if heldObject.uid == 0 then
|
||||
if mouse2 then
|
||||
--[[
|
||||
local center = transform.position + cameraTransform.position
|
||||
local direction = transform.forward + Vector3f( 0, cameraTransform.forward.y, 0 )
|
||||
direction = direction:normalize() * 4
|
||||
]]
|
||||
local center = flattenedTransform.position
|
||||
local direction = flattenedTransform.forward * pullDistance
|
||||
local prop, depth = physicsBody:rayCast( center, direction )
|
||||
if depth >= 0 and prop and not string.matched( prop:name(), "/^worldspawn/" ) then
|
||||
local heldObjectTransform = prop:getComponent("Transform")
|
||||
local heldObjectPhysicsBody = prop:getComponent("PhysicsBody")
|
||||
|
||||
local strength = 500
|
||||
local distanceSquared = (heldObjectTransform.position - flattenedTransform.position):magnitude()
|
||||
|
||||
heldObjectPhysicsBody:applyImpulse( flattenedTransform.forward * -heldObjectPhysicsBody:getMass() * strength / distanceSquared )
|
||||
if timers.physcannon:elapsed() > 1.0 then
|
||||
timers.physcannon:reset()
|
||||
|
||||
playSound("phys_tooHeavy")
|
||||
end
|
||||
end
|
||||
end
|
||||
else
|
||||
|
||||
if wheel ~= 0 then
|
||||
heldObject.distance = heldObject.distance + (wheel / 120 * heldObject.scrollSpeed) * time.delta()
|
||||
end
|
||||
if mouse3 then
|
||||
heldObject.rotate = not heldObject.rotate
|
||||
end
|
||||
|
||||
local prop = entities.get( heldObject.uid )
|
||||
local heldObjectTransform = prop:getComponent("Transform")
|
||||
local heldObjectPhysicsBody = prop:getComponent("PhysicsBody")
|
||||
|
||||
if mouse1 and timers.physcannon:elapsed() > 0.5 then
|
||||
timers.physcannon:reset()
|
||||
|
||||
heldObject.uid = 0
|
||||
heldObjectPhysicsBody:enableGravity(true)
|
||||
heldObjectPhysicsBody:applyImpulse( flattenedTransform.forward * heldObjectPhysicsBody:getMass() * 50 )
|
||||
|
||||
playSound("phys_launch"..math.random(1,4))
|
||||
else
|
||||
if heldObject.rotate then
|
||||
heldObjectTransform.orientation = Quaternion.lookAt( (heldObjectTransform.position - flattenedTransform.position):normalize(), transform.up )
|
||||
end
|
||||
|
||||
local forward = flattenedTransform.forward * heldObject.distance --flattenedTransform.orientation:rotate( Vector3f(0,0,1) )
|
||||
if heldObject.smoothSpeed ~= 0 then
|
||||
local heldObjectFlattened = heldObjectTransform:flatten()
|
||||
|
||||
local target = flattenedTransform.position + forward
|
||||
local offset = target - heldObjectFlattened.position
|
||||
|
||||
local stiffness = 15.0
|
||||
local damping = 2.0
|
||||
local currentVelocity = heldObjectPhysicsBody:getVelocity()
|
||||
local mass = heldObjectPhysicsBody:getMass()
|
||||
|
||||
local springForce = offset * stiffness
|
||||
local dampingForce = currentVelocity * -damping
|
||||
|
||||
heldObjectPhysicsBody:applyImpulse((springForce + dampingForce) * mass * time.delta())
|
||||
else
|
||||
heldObjectTransform.position = flattenedTransform.position + forward
|
||||
end
|
||||
end
|
||||
end
|
||||
end )
|
||||
-- on use
|
||||
ent:addHook( "entity:Use.%UID%", function( payload )
|
||||
if payload.user ~= ent:uid() then return end
|
||||
|
||||
local function onUse( payload )
|
||||
local validUse = false
|
||||
|
||||
-- not currently holding anything, and hit something
|
||||
if heldObject.uid == 0 and payload.depth > 0 then
|
||||
local prop = entities.get( payload.uid )
|
||||
local propMetadata = prop:getComponent("Metadata")
|
||||
-- entity is holdable, pick it up
|
||||
if propMetadata["holdable"] then
|
||||
validUse = true
|
||||
local heldObjectTransform = prop:getComponent("Transform")
|
||||
@ -246,11 +113,10 @@ ent:addHook( "entity:Use.%UID%", function( payload )
|
||||
|
||||
local heldObjectPhysicsBody = prop:getComponent("PhysicsBody")
|
||||
heldObjectPhysicsBody:enableGravity(false)
|
||||
|
||||
print( prop:name() )
|
||||
else
|
||||
validUse = not string.matched( prop:name(), "/^worldspawn/" )
|
||||
end
|
||||
-- currently holding something, drop it
|
||||
elseif heldObject.uid ~= 0 then
|
||||
validUse = true
|
||||
local prop = entities.get( heldObject.uid )
|
||||
@ -263,9 +129,149 @@ ent:addHook( "entity:Use.%UID%", function( payload )
|
||||
heldObject.momentum = Vector3f(0,0,0)
|
||||
end
|
||||
|
||||
if validUse then
|
||||
playSound("select")
|
||||
else
|
||||
playSound("deny")
|
||||
playSound(validUse and "select" or "deny")
|
||||
end
|
||||
|
||||
local function tickUse( transform, inputs )
|
||||
-- trigger use
|
||||
if timers.use:elapsed() > 0.5 and inputs["E"] then
|
||||
timers.use:reset()
|
||||
local center = transform.position
|
||||
local direction = transform.forward * useDistance
|
||||
local prop, depth = physicsBody:rayCast(center, direction)
|
||||
|
||||
local payload = {
|
||||
user = ent:uid(),
|
||||
uid = prop and prop:uid() or 0,
|
||||
depth = depth,
|
||||
}
|
||||
if prop then prop:lazyCallHook("entity:Use.%UID%", payload) end
|
||||
ent:lazyCallHook("entity:Use.%UID%", payload)
|
||||
end
|
||||
end
|
||||
|
||||
local function tickGravGun( transform, inputs )
|
||||
-- not holding anything
|
||||
if heldObject.uid == 0 then
|
||||
-- try and launch object in sights
|
||||
if inputs["mouse2"] then
|
||||
local center = transform.position
|
||||
local direction = transform.forward * pullDistance
|
||||
local prop, depth = physicsBody:rayCast( center, direction )
|
||||
if depth >= 0 and prop and not string.matched( prop:name(), "/^worldspawn/" ) then
|
||||
local heldObjectTransform = prop:getComponent("Transform")
|
||||
local heldObjectPhysicsBody = prop:getComponent("PhysicsBody")
|
||||
|
||||
local strength = 500
|
||||
local distanceSquared = (heldObjectTransform.position - transform.position):magnitude()
|
||||
|
||||
heldObjectPhysicsBody:applyImpulse( transform.forward * -heldObjectPhysicsBody:getMass() * strength / distanceSquared )
|
||||
if timers.physcannon:elapsed() > 1.0 then
|
||||
timers.physcannon:reset()
|
||||
|
||||
playSound("phys_tooHeavy")
|
||||
end
|
||||
end
|
||||
end
|
||||
-- holding something
|
||||
else
|
||||
-- adjust hold distance
|
||||
if inputs["wheel"] ~= 0 then
|
||||
heldObject.distance = heldObject.distance + (inputs["wheel"] / 120 * heldObject.scrollSpeed) * time.delta()
|
||||
end
|
||||
-- update rotation mode
|
||||
if inputs["mouse3"] then
|
||||
heldObject.rotate = not heldObject.rotate
|
||||
end
|
||||
|
||||
local prop = entities.get( heldObject.uid )
|
||||
local heldObjectTransform = prop:getComponent("Transform")
|
||||
local heldObjectPhysicsBody = prop:getComponent("PhysicsBody")
|
||||
|
||||
-- launch held object
|
||||
if inputs["mouse1"] and timers.physcannon:elapsed() > 0.5 then
|
||||
timers.physcannon:reset()
|
||||
|
||||
heldObject.uid = 0
|
||||
heldObjectPhysicsBody:enableGravity(true)
|
||||
heldObjectPhysicsBody:applyImpulse( transform.forward * heldObjectPhysicsBody:getMass() * 50 )
|
||||
|
||||
playSound("phys_launch"..math.random(1,4))
|
||||
else
|
||||
-- update rotation
|
||||
if heldObject.rotate then
|
||||
heldObjectTransform.orientation = Quaternion.lookAt( (heldObjectTransform.position - transform.position):normalize(), transform.up )
|
||||
end
|
||||
|
||||
-- move held object
|
||||
local forward = transform.forward * heldObject.distance
|
||||
if heldObject.smoothSpeed ~= 0 then
|
||||
local heldObjectFlattened = heldObjectTransform:flatten()
|
||||
|
||||
local target = transform.position + forward
|
||||
local offset = target - heldObjectFlattened.position
|
||||
|
||||
local stiffness = 15.0
|
||||
local damping = 2.0
|
||||
local currentVelocity = heldObjectPhysicsBody:getVelocity()
|
||||
local mass = heldObjectPhysicsBody:getMass()
|
||||
|
||||
local springForce = offset * stiffness
|
||||
local dampingForce = currentVelocity * -damping
|
||||
|
||||
heldObjectPhysicsBody:applyImpulse((springForce + dampingForce) * mass * time.delta())
|
||||
else
|
||||
heldObjectTransform.position = transform.position + forward
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
-- on tick
|
||||
ent:bind( "tick", function(self)
|
||||
local inControl = scene:globalFindByName("Gui: Menu"):uid() == 0
|
||||
|
||||
local inputs = {
|
||||
E = inputs.key("E") or inputs.key("R_Y"),
|
||||
F = inputs.key("F"),
|
||||
mouse1 = inputs.key("Mouse1") or inputs.key("L_TRIGGER"),
|
||||
mouse2 = inputs.key("Mouse2") or inputs.key("R_TRIGGER"),
|
||||
mouse3 = inputs.key("Mouse3"),
|
||||
wheel = inputs.analog("MouseWheel"),
|
||||
}
|
||||
|
||||
if not inControl then
|
||||
inputs["E"] = false
|
||||
inputs["F"] = false
|
||||
inputs["mouse1"] = false
|
||||
inputs["mouse2"] = false
|
||||
inputs["mouse3"] = false
|
||||
inputs["wheel"] = 0
|
||||
end
|
||||
|
||||
-- eye transform
|
||||
local flattenedTransform = fixedCamera and transform:flatten() or cameraTransform:flatten()
|
||||
|
||||
-- update flashlight
|
||||
tickFlashlight( flattenedTransform, inputs )
|
||||
|
||||
-- update use
|
||||
tickUse( flattenedTransform, inputs )
|
||||
|
||||
-- update HOLP
|
||||
tickGravGun( flattenedTransform, inputs )
|
||||
|
||||
-- get collision events
|
||||
--[[
|
||||
local collisionEvents = physicsBody:getCollisionEvents()
|
||||
for i, event in ipairs(collisionEvents) do
|
||||
print( event.state, event.a, event.b, event.point, event.normal, event.impulse )
|
||||
end
|
||||
]]
|
||||
end )
|
||||
|
||||
-- on use
|
||||
ent:addHook( "entity:Use.%UID%", function( payload )
|
||||
if payload.user ~= ent:uid() then return end
|
||||
onUse( payload )
|
||||
end )
|
||||
@ -14,6 +14,7 @@
|
||||
|
||||
namespace impl {
|
||||
void solveConstraint( pod::Constraint& constraint, float dt );
|
||||
void solveConstraints( uf::stl::vector<pod::Constraint*>& constraint, float dt );
|
||||
}
|
||||
|
||||
namespace uf {
|
||||
|
||||
@ -4,19 +4,21 @@
|
||||
|
||||
namespace impl {
|
||||
void bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt = 0 );
|
||||
bool generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
|
||||
bool generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
|
||||
void computeLocalContacts( pod::Manifold& manifold );
|
||||
bool generateManifoldGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
|
||||
bool generateManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
|
||||
void computeLocalManifold( pod::Manifold& manifold );
|
||||
bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSqThreshold = 1.0e-2f, float normThreshold = 0.9f );
|
||||
void reduceContacts( pod::Manifold& manifold );
|
||||
void mergeContacts( pod::Manifold& manifold );
|
||||
void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f );
|
||||
void reduceManifold( pod::Manifold& manifold );
|
||||
void mergeManifold( pod::Manifold& manifold );
|
||||
void retrieveManifold( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f );
|
||||
void prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies );
|
||||
void updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache );
|
||||
void pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache );
|
||||
void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt );
|
||||
void warmupContact( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt );
|
||||
void warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt );
|
||||
|
||||
void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
|
||||
void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
|
||||
void solveManifold( uf::stl::vector<pod::Manifold>& manifolds, float dt );
|
||||
|
||||
void dispatchManifold( pod::Manifold& manifold, pod::CollisionEvent::events_t& events, pod::CollisionEvent::map_t& active, const pod::CollisionEvent::map_t& previous );
|
||||
}
|
||||
@ -55,6 +55,8 @@ namespace uf {
|
||||
void UF_API applyRotation( pod::PhysicsBody& body, const pod::Vector3f& axis, float angle );
|
||||
|
||||
pod::World& UF_API getWorld();
|
||||
const pod::CollisionEvent::events_t& UF_API getCollisionEvents( const pod::World& world );
|
||||
pod::CollisionEvent::events_t UF_API getCollisionEvents( const pod::PhysicsBody& body );
|
||||
|
||||
pod::PhysicsBody& UF_API create( uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
|
||||
@ -376,6 +376,25 @@ namespace pod {
|
||||
static constexpr float linearSleepEpsilon = 0.2f; // m/s
|
||||
static constexpr float angularSleepEpsilon = 0.2f; // rad/s
|
||||
};
|
||||
|
||||
enum class CollisionState {
|
||||
ENTER,
|
||||
SUSTAIN,
|
||||
EXIT
|
||||
};
|
||||
|
||||
struct CollisionEvent {
|
||||
typedef std::pair<pod::PhysicsBody*, pod::PhysicsBody*> pairs_t;
|
||||
typedef uf::stl::vector<pod::CollisionEvent> events_t;
|
||||
typedef uf::stl::unordered_map<uint64_t, pod::CollisionEvent::pairs_t> map_t;
|
||||
|
||||
pod::CollisionState state = {};
|
||||
pod::PhysicsBody* a = NULL;
|
||||
pod::PhysicsBody* b = NULL;
|
||||
pod::Vector3f point = {};
|
||||
pod::Vector3f normal = {};
|
||||
float impulse = 0;
|
||||
};
|
||||
}
|
||||
|
||||
namespace pod {
|
||||
@ -473,6 +492,9 @@ namespace pod {
|
||||
pod::Vector3f gravity = { 0, -9.81f, 0 };
|
||||
pod::BVH dynamicBvh;
|
||||
pod::BVH staticBvh;
|
||||
|
||||
pod::CollisionEvent::events_t collisionEvents;
|
||||
pod::CollisionEvent::map_t activeCollisions;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -573,6 +573,16 @@ void ext::PlayerBehavior::tick( uf::Object& self ) {
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
{
|
||||
int count = 0;
|
||||
auto events = uf::physics::getCollisionEvents( physicsBody );
|
||||
for ( const auto& event : events ) {
|
||||
// do something
|
||||
}
|
||||
UF_MSG_DEBUG("count={}", events.size());
|
||||
}
|
||||
#endif
|
||||
#if UF_USE_OPENAL
|
||||
if ( stats.floored && !stats.noclipped ) {
|
||||
if ( stats.walking ) {
|
||||
|
||||
@ -36,6 +36,11 @@ namespace binds {
|
||||
return uf::transform::flatten( t );
|
||||
}
|
||||
|
||||
auto getCollisionEvents( const pod::PhysicsBody& body ) {
|
||||
auto events = uf::physics::getCollisionEvents( body );
|
||||
return sol::as_table( events );
|
||||
}
|
||||
|
||||
std::tuple<uf::Object*, float> rayCast( pod::PhysicsBody& self, const pod::Vector3f& center, const pod::Vector3f& direction ) {
|
||||
pod::RayQuery query = uf::physics::rayCast( pod::Ray{center, uf::vector::normalize( direction )}, self, uf::vector::norm( direction ) );
|
||||
uf::Object* object = query.hit ? query.body->object : NULL;
|
||||
@ -102,6 +107,14 @@ namespace binds {
|
||||
|
||||
#include <uf/ext/lua/component.h>
|
||||
|
||||
UF_LUA_REGISTER_USERTYPE_AND_COMPONENT(pod::CollisionEvent,
|
||||
UF_LUA_REGISTER_USERTYPE_MEMBER(pod::CollisionEvent::state),
|
||||
UF_LUA_REGISTER_USERTYPE_MEMBER(pod::CollisionEvent::a),
|
||||
UF_LUA_REGISTER_USERTYPE_MEMBER(pod::CollisionEvent::b),
|
||||
UF_LUA_REGISTER_USERTYPE_MEMBER(pod::CollisionEvent::point),
|
||||
UF_LUA_REGISTER_USERTYPE_MEMBER(pod::CollisionEvent::normal),
|
||||
UF_LUA_REGISTER_USERTYPE_MEMBER(pod::CollisionEvent::impulse)
|
||||
)
|
||||
UF_LUA_REGISTER_USERTYPE_AND_COMPONENT(pod::AABB,
|
||||
sol::call_constructor, sol::initializers(
|
||||
[]( pod::AABB& self ) {
|
||||
@ -206,6 +219,7 @@ UF_LUA_REGISTER_USERTYPE_AND_COMPONENT(pod::PhysicsBody,
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( applyImpulse, UF_LUA_C_FUN(uf::physics::applyImpulse) ),
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( applyRotation, UF_LUA_C_FUN(::binds::body::applyRotation) ),
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( getTransform, UF_LUA_C_FUN(::binds::body::getTransform) ),
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( getCollisionEvents, UF_LUA_C_FUN(::binds::body::getCollisionEvents) ),
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( setGravity, UF_LUA_C_FUN(::binds::body::setGravity) ),
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( enableGravity, UF_LUA_C_FUN(::binds::body::enableGravity) ),
|
||||
UF_LUA_REGISTER_USERTYPE_DEFINE( rayCast, UF_LUA_C_FUN(::binds::body::rayCast) ),
|
||||
|
||||
@ -1,6 +1,11 @@
|
||||
#include <uf/utils/math/physics/common.h>
|
||||
#include <uf/utils/math/physics/constraints.h>
|
||||
|
||||
void impl::solveConstraints( uf::stl::vector<pod::Constraint*>& constraints, float dt ) {
|
||||
for ( uint32_t i = 0; i < uf::physics::settings.solverIterations; ++i ) {
|
||||
for ( auto* constraint : constraints ) impl::solveConstraint( *constraint, dt );
|
||||
}
|
||||
}
|
||||
void impl::solveConstraint( pod::Constraint& constraint, float dt ) {
|
||||
switch ( constraint.type ) {
|
||||
case pod::ConstraintType::BALL_AND_SOCKET: {
|
||||
|
||||
@ -11,7 +11,7 @@ void impl::bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold
|
||||
manifold.points.reserve(4);
|
||||
}
|
||||
|
||||
bool impl::generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
bool impl::generateManifoldGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
impl::bindManifold( a, b, manifold, dt );
|
||||
|
||||
pod::Simplex simplex;
|
||||
@ -23,11 +23,11 @@ bool impl::generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::M
|
||||
return true;
|
||||
}
|
||||
|
||||
bool impl::generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
bool impl::generateManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
bool useGjk = uf::physics::settings.useGjk;
|
||||
if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) useGjk = false;
|
||||
//if ( a.collider.type == pod::ShapeType::PLANE || b.collider.type == pod::ShapeType::PLANE ) useGjk = false;
|
||||
if ( useGjk ) return generateContactsGjk( a, b, manifold, dt );
|
||||
if ( useGjk ) return generateManifoldGjk( a, b, manifold, dt );
|
||||
impl::bindManifold( a, b, manifold, dt );
|
||||
|
||||
#define CHECK_CONTACT( A, B, fun )\
|
||||
@ -94,7 +94,7 @@ bool impl::generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Mani
|
||||
return false;
|
||||
}
|
||||
|
||||
void impl::computeLocalContacts( pod::Manifold& manifold ) {
|
||||
void impl::computeLocalManifold( pod::Manifold& manifold ) {
|
||||
if ( manifold.points.empty() ) return;
|
||||
|
||||
auto& a = *manifold.a;
|
||||
@ -113,7 +113,7 @@ bool impl::similarContact( const pod::Contact& a, const pod::Contact& b, float d
|
||||
return uf::vector::distanceSquared(a.point, b.point) < distSqThreshold && uf::vector::dot(a.normal, b.normal) > normThreshold;
|
||||
}
|
||||
|
||||
void impl::reduceContacts( pod::Manifold& manifold ) {
|
||||
void impl::reduceManifold( pod::Manifold& manifold ) {
|
||||
if ( manifold.points.size() <= 4 ) return;
|
||||
|
||||
#if 1
|
||||
@ -210,7 +210,7 @@ void impl::reduceContacts( pod::Manifold& manifold ) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void impl::mergeContacts( pod::Manifold& manifold ) {
|
||||
void impl::mergeManifold( pod::Manifold& manifold ) {
|
||||
STATIC_THREAD_LOCAL(uf::stl::vector<pod::Contact>, result);
|
||||
result.reserve(4);
|
||||
|
||||
@ -231,7 +231,7 @@ void impl::mergeContacts( pod::Manifold& manifold ) {
|
||||
manifold.points = result;
|
||||
}
|
||||
|
||||
void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold, float separationThreshold, float decay ) {
|
||||
void impl::retrieveManifold( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold, float separationThreshold, float decay ) {
|
||||
auto& a = *current.a;
|
||||
auto& b = *current.b;
|
||||
|
||||
@ -320,7 +320,7 @@ void impl::pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& ca
|
||||
}
|
||||
}
|
||||
|
||||
void impl::warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) {
|
||||
void impl::warmupContact( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) {
|
||||
if ( !c.lifetime ) return; // too new
|
||||
|
||||
// build relative offsets
|
||||
@ -337,7 +337,7 @@ void impl::warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::
|
||||
}
|
||||
void impl::warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt ) {
|
||||
for ( auto& contact : manifold.points ) {
|
||||
impl::warmupContacts( a, b, contact, dt );
|
||||
impl::warmupContact( a, b, contact, dt );
|
||||
}
|
||||
}
|
||||
|
||||
@ -348,7 +348,25 @@ void impl::resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manif
|
||||
for ( auto& contact : manifold.points ) impl::iterativeImpulseSolver( a, b, contact, dt );
|
||||
}
|
||||
|
||||
void impl::solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt ) {
|
||||
void impl::solveManifold( uf::stl::vector<pod::Manifold>& manifolds, float dt ) {
|
||||
if ( uf::physics::settings.warmupSolver ) for ( auto& manifold : manifolds ) impl::warmupManifold( *manifold.a, *manifold.b, manifold, dt );
|
||||
for ( auto i = 0; i < uf::physics::settings.solverIterations; ++i ) for ( auto& manifold : manifolds ) impl::resolveManifold( *manifold.a, *manifold.b, manifold, dt );
|
||||
}
|
||||
|
||||
void impl::dispatchManifold( pod::Manifold& manifold, pod::CollisionEvent::events_t& events, pod::CollisionEvent::map_t& active, const pod::CollisionEvent::map_t& previous ) {
|
||||
auto pairKey = impl::makePairKey( *manifold.a, *manifold.b );
|
||||
// find largest impulse
|
||||
float maxImpulse = 0.0f;
|
||||
for ( const auto& c : manifold.points ) maxImpulse = std::max( maxImpulse, c.accumulatedNormalImpulse );
|
||||
// mark as an active collision
|
||||
active[pairKey] = { manifold.a, manifold.b };
|
||||
// dispatch
|
||||
events.emplace_back(pod::CollisionEvent{
|
||||
.state = previous.count( pairKey ) == 0 ? pod::CollisionState::ENTER : pod::CollisionState::SUSTAIN,
|
||||
.a = manifold.a,
|
||||
.b = manifold.b,
|
||||
.point = manifold.points[0].point,
|
||||
.normal = manifold.points[0].normal,
|
||||
.impulse = maxImpulse,
|
||||
});
|
||||
}
|
||||
@ -90,6 +90,14 @@ void uf::physics::step( pod::World& world, float dt ) {
|
||||
auto& constraints = world.constraints;
|
||||
auto& dynamicBvh = world.dynamicBvh;
|
||||
auto& staticBvh = world.staticBvh;
|
||||
auto& activeCollisions = world.activeCollisions;
|
||||
auto& collisionEvents = world.collisionEvents;
|
||||
|
||||
STATIC_THREAD_LOCAL(pod::CollisionEvent::events_t, previousCollisionEvents);
|
||||
STATIC_THREAD_LOCAL(pod::CollisionEvent::map_t, previousCollisions);
|
||||
|
||||
std::swap( previousCollisions, activeCollisions );
|
||||
std::swap( previousCollisionEvents, collisionEvents );
|
||||
|
||||
if ( bodies.empty() ) return;
|
||||
|
||||
@ -148,10 +156,10 @@ void uf::physics::step( pod::World& world, float dt ) {
|
||||
|
||||
pod::Manifold manifold;
|
||||
// did not collide
|
||||
if ( !impl::generateContacts( a, b, manifold, dt ) ) continue;
|
||||
if ( !impl::generateManifold( a, b, manifold, dt ) ) continue;
|
||||
|
||||
// compute local points (for reprojection)
|
||||
impl::computeLocalContacts( manifold );
|
||||
impl::computeLocalManifold( manifold );
|
||||
|
||||
// bodies with meshes already reorient the normal to the triangle's center
|
||||
bool shouldReorient = true;
|
||||
@ -165,14 +173,14 @@ void uf::physics::step( pod::World& world, float dt ) {
|
||||
// retrieve accumulated impulses
|
||||
if ( uf::physics::settings.warmupSolver ) {
|
||||
auto it = uf::physics::settings.manifoldsCache.find( impl::makePairKey( a, b ) );
|
||||
if ( it != uf::physics::settings.manifoldsCache.end() ) impl::retrieveContacts( manifold, it->second );
|
||||
if ( it != uf::physics::settings.manifoldsCache.end() ) impl::retrieveManifold( manifold, it->second );
|
||||
}
|
||||
// merge similar contacts from a mesh to ensure continuity
|
||||
if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) {
|
||||
impl::mergeContacts( manifold );
|
||||
impl::mergeManifold( manifold );
|
||||
}
|
||||
// keep four most important contacts
|
||||
impl::reduceContacts( manifold );
|
||||
impl::reduceManifold( manifold );
|
||||
// no points remained, skip
|
||||
if ( manifold.points.empty() ) continue;
|
||||
// wake up bodies
|
||||
@ -188,36 +196,42 @@ void uf::physics::step( pod::World& world, float dt ) {
|
||||
}
|
||||
|
||||
// store manifold
|
||||
manifolds.emplace_back(manifold);
|
||||
manifolds.emplace_back( manifold );
|
||||
}
|
||||
|
||||
// pass manifolds to solver
|
||||
impl::solveContacts( manifolds, dt );
|
||||
impl::solveManifold( manifolds, dt );
|
||||
// do position correction
|
||||
impl::solvePositions( manifolds, dt );
|
||||
// solve constraints
|
||||
for ( uint32_t i = 0; i < uf::physics::settings.solverIterations; ++i ) {
|
||||
for ( auto& constraint : constraints ) {
|
||||
impl::solveConstraint( *constraint, dt );
|
||||
}
|
||||
}
|
||||
impl::solveConstraints( constraints, dt );
|
||||
// cache manifold positions
|
||||
if ( uf::physics::settings.warmupSolver ) {
|
||||
impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
|
||||
}
|
||||
if ( uf::physics::settings.warmupSolver ) impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
|
||||
});
|
||||
uf::thread::execute( tasks );
|
||||
|
||||
// prune expired manifolds in the cache
|
||||
if ( uf::physics::settings.warmupSolver ) impl::pruneManifoldCache( uf::physics::settings.manifoldsCache );
|
||||
|
||||
if ( uf::physics::settings.debugDraw ) {
|
||||
for ( auto& island : islands ) for ( auto& manifold : island.manifolds ) impl::drawManifold( manifold );
|
||||
for ( auto& island : islands ) for ( auto& manifold : island.manifolds ) {
|
||||
// dispatch collision events
|
||||
impl::dispatchManifold( manifold, collisionEvents, activeCollisions, previousCollisions );
|
||||
// draw collision events
|
||||
if ( uf::physics::settings.debugDraw ) impl::drawManifold( manifold );
|
||||
}
|
||||
// dispatch exiting collisions
|
||||
for ( auto& [ key, pair ] : previousCollisions ) {
|
||||
// sustained collision
|
||||
if ( activeCollisions.count( key ) > 0 ) continue;
|
||||
// mark as exiting
|
||||
collisionEvents.emplace_back(pod::CollisionEvent{
|
||||
.state = pod::CollisionState::EXIT,
|
||||
.a = pair.first,
|
||||
.b = pair.second,
|
||||
});
|
||||
}
|
||||
|
||||
for ( auto* b : bodies ) {
|
||||
if ( b->inverseMass == 0.0f ) continue;
|
||||
impl::snapVelocity( *b, dt );
|
||||
}
|
||||
// snap velocities of bodies
|
||||
for ( auto* b : bodies ) impl::snapVelocity( *b, dt );
|
||||
}
|
||||
|
||||
void uf::physics::setMass( pod::PhysicsBody& body, float mass ) {
|
||||
@ -447,7 +461,18 @@ pod::World& uf::physics::getWorld() {
|
||||
auto& scene = uf::scene::getCurrentScene();
|
||||
return scene.getComponent<pod::World>();
|
||||
}
|
||||
|
||||
const pod::CollisionEvent::events_t& uf::physics::getCollisionEvents( const pod::World& world ) {
|
||||
return world.collisionEvents;
|
||||
}
|
||||
pod::CollisionEvent::events_t uf::physics::getCollisionEvents( const pod::PhysicsBody& body ) {
|
||||
const auto& events = uf::physics::getCollisionEvents( *body.world );
|
||||
pod::CollisionEvent::events_t res;
|
||||
for ( auto& event : events ) {
|
||||
if ( event.a != &body && event.b != &body ) continue;
|
||||
res.emplace_back( event );
|
||||
}
|
||||
return res;
|
||||
}
|
||||
// body creation
|
||||
pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, float mass, const pod::Vector3f& offset ) {
|
||||
auto& root = object.getComponent<pod::PhysicsBody>();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user