ecker/engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

even more inane cleanup, and even more constraints

Browse Source
This commit is contained in:
ecker 2026-05-22 00:02:37 -05:00
parent 7758389c6c
commit 40157df205
60 changed files with 886 additions and 237 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
bin/data/config.json
View File

@ -350,7 +350,7 @@
"max": 0.01 // 0.2
},
"debug draw": {
"dynamic": true
"dynamic": false
},
"fixed step": true,
"substeps": 4

2
bin/data/entities/ball.json
View File

@ -14,7 +14,7 @@
"metadata": {
"holdable": true,
"physics": {
"mass": 10,
"mass": 1,
"type": "sphere",
"radius": 1,
"min": [ -0.5, -0.5, -0.5 ],

2
bin/data/entities/physics_prop.json
View File

@ -6,7 +6,7 @@
"metadata": {
"holdable": true,
"physics": {
"mass": 100,
"mass": 1900,
// "inertia": false,
"type": "obb"
// "type": "mesh"

2
bin/data/entities/prop.json
View File

@ -7,7 +7,7 @@
"holdable": true,
"physics": {
"mass": 0,
"inertia": false,
// "inertia": false,
// "type": "bounding box"
"type": "mesh"
// "type": "hull"

2
engine/inc/uf/utils/math/physics/broadphase.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "impl.h"
#include "structs.h"
#include "broadphase/bvh.h"
#include "broadphase/island.h"

2
engine/inc/uf/utils/math/physics/broadphase/bvh.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
pod::BVH::index_t buildBVHNode( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, pod::BVH::index_t start, pod::BVH::index_t end, pod::BVH::index_t capacity = 2 );

2
engine/inc/uf/utils/math/physics/broadphase/island.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, const uf::stl::vector<pod::Constraint*>& constraints, uf::stl::vector<pod::Island>& islands );

2
engine/inc/uf/utils/math/physics/common.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "impl.h"
#include "structs.h"
#include "draw.h"
// to-do: organize this mess

7
engine/inc/uf/utils/math/physics/constraints.h
View File

@ -1,11 +1,16 @@
#pragma once
#include "impl.h"
#include "structs.h"
#include "constraints/contact.h"
#include "constraints/ballSocket.h"
#include "constraints/hinge.h"
#include "constraints/coneTwist.h"
#include "constraints/slider.h"
#include "constraints/distance.h"
#include "constraints/weld.h"
#include "constraints/spring.h"
#include "constraints/motor.h"
namespace impl {
void solveConstraint( pod::Constraint& constraint, float dt );

2
engine/inc/uf/utils/math/physics/constraints/ballSocket.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void solveBallSocketConstraint( pod::Constraint& constraint, float dt );

2
engine/inc/uf/utils/math/physics/constraints/coneTwist.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void solveConeTwistConstraint( pod::Constraint& constraint, float dt );

2
engine/inc/uf/utils/math/physics/constraints/contact.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt = 0 );

17
engine/inc/uf/utils/math/physics/constraints/distance.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
#include "../structs.h"
namespace impl {
void solveDistanceConstraint( pod::Constraint& constraint, float dt );
}
/*
namespace uf {
namespace physics {
pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
}
}
*/

2
engine/inc/uf/utils/math/physics/constraints/hinge.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void solveHingeConstraint( pod::Constraint& constraint, float dt );

17
engine/inc/uf/utils/math/physics/constraints/motor.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
#include "../structs.h"
namespace impl {
void solve1DLinearMotor(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& axis,
float targetVelocity, float maxForce, float& accumulatedImpulse, float dt
);
void solve1DAngularMotor(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& axis,
float targetVelocity, float maxTorque,
float& accumulatedImpulse, float dt
);
}

17
engine/inc/uf/utils/math/physics/constraints/slider.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
#include "../structs.h"
namespace impl {
void solveSliderConstraint( pod::Constraint& constraint, float dt );
}
/*
namespace uf {
namespace physics {
pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
}
}
*/

17
engine/inc/uf/utils/math/physics/constraints/spring.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
#include "../structs.h"
namespace impl {
void solveSpringConstraint( pod::Constraint& constraint, float dt );
}
/*
namespace uf {
namespace physics {
pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
}
}
*/

17
engine/inc/uf/utils/math/physics/constraints/weld.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
#include "../structs.h"
namespace impl {
void solveWeldConstraint( pod::Constraint& constraint, float dt );
}
/*
namespace uf {
namespace physics {
pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
}
}
*/

2
engine/inc/uf/utils/math/physics/draw.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "impl.h"
#include "structs.h"
namespace impl {
struct Vertex {

22
engine/inc/uf/utils/math/physics/impl.h
View File

@ -3,31 +3,13 @@
#include <uf/config.h>
#include <uf/utils/time/time.h>
#include <uf/utils/math/quant.h>
#include <cfloat>
#include "structs.h"
#define EPS 1.0e-6f
#define EPS2 (EPS * EPS)
#define ASSERT_COLLIDER_TYPES( A, B ) UF_ASSERT( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B );
#define REORIENT_NORMALS_ON_FETCH 0
#define REORIENT_NORMALS_ON_CONTACT 1
#define REVERSE_COLLIDER( a, b, fun )\
auto start = manifold.points.size();\
if ( !::fun( b, a, manifold ) ) return false;\
for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\
return true;
#include "narrowphase.h"
#include "constraints.h"
namespace uf {
namespace physics {
typedef pod::Math::num_t num_t;
namespace time = uf::time; // to-do: have separate values from the physics system
extern UF_API pod::World world;
extern UF_API pod::PhysicsSettings settings;
void UF_API initialize();
void UF_API initialize( uf::Object& );
void UF_API initialize( pod::World& );

8
engine/inc/uf/utils/math/physics/integration.h
View File

@ -1,9 +1,15 @@
#pragma once
#include "impl.h"
#include "structs.h"
namespace impl {
/*FORCE_INLINE*/ pod::SolverBodyContext solverBodyContext( const pod::PhysicsBody& body );
float computeEffectiveMass( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& row );
float computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& n );
float computeMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& rowI, const pod::JacobianRow& rowJ );
float computeAngularMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::Vector3f& n_i, const pod::Vector3f& n_j );
void applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse );
void applyImpulseTo(
pod::PhysicsBody& a, pod::PhysicsBody& b,

2
engine/inc/uf/utils/math/physics/narrowphase.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "impl.h"
#include "structs.h"
#include "narrowphase/aabb.h"
#include "narrowphase/obb.h"

2
engine/inc/uf/utils/math/physics/narrowphase/aabb.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/capsule.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool capsuleAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/epa.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void addOrRemoveBorder( uf::stl::vector<std::pair<pod::SupportPoint, pod::SupportPoint>>& edges, std::pair<pod::SupportPoint, pod::SupportPoint> e);

2
engine/inc/uf/utils/math/physics/narrowphase/gjk.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
pod::Vector3f support( const pod::PhysicsBody& body, const pod::Vector3f& dir );

2
engine/inc/uf/utils/math/physics/narrowphase/hull.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool hullAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/mesh.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool meshAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/obb.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool obbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/plane.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool planeAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/ray.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool rayTriangleIntersect( const pod::Ray& ray, const pod::Triangle& tri, float& t, float& u, float& v );

2
engine/inc/uf/utils/math/physics/narrowphase/sphere.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool sphereAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/narrowphase/triangle.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold );

2
engine/inc/uf/utils/math/physics/solvers.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "impl.h"
#include "structs.h"
#include "constraints/contact.h"

2
engine/inc/uf/utils/math/physics/solvers/block.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
/*FORCE_INLINE*/ bool block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );

2
engine/inc/uf/utils/math/physics/solvers/iterativeImpulse.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "../impl.h"
#include "../structs.h"
namespace impl {
void iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt );

113
engine/inc/uf/utils/math/physics/structs.h
View File

@ -1,3 +1,5 @@
#pragma once
#include <uf/utils/math/vector.h>
#include <uf/utils/math/quaternion.h>
#include <uf/utils/math/matrix.h>
@ -12,6 +14,22 @@
#include <uf/engine/object/object.h>
#include <uf/utils/mesh/mesh.h>
#include <cfloat>
#define EPS 1.0e-6f
#define EPS2 (EPS * EPS)
#define ASSERT_COLLIDER_TYPES( A, B ) UF_ASSERT( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B );
#define REORIENT_NORMALS_ON_FETCH 0
#define REORIENT_NORMALS_ON_CONTACT 1
#define REVERSE_COLLIDER( a, b, fun )\
auto start = manifold.points.size();\
if ( !::fun( b, a, manifold ) ) return false;\
for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\
return true;
// Forward declates
namespace pod {
struct World;
@ -43,6 +61,10 @@ namespace pod {
BALL_AND_SOCKET,
HINGE,
CONE_TWIST,
SLIDER,
DISTANCE,
WELD,
SPRING,
//CONTACT
};
@ -82,6 +104,68 @@ namespace pod {
float twistLimit; // M_PI / 8.0f;
};
struct Slider {
pod::Vector3f localAnchorA;
pod::Vector3f localAnchorB;
pod::Vector2f accumulatedLinearImpulse;
pod::Vector3f accumulatedAngularImpulse;
pod::Vector3f localAxisA;
pod::Vector3f localAxisB;
pod::Vector3f localReferenceAxisA;
pod::Vector3f localReferenceAxisB;
float lowerLimit;
float upperLimit;
float accumulatedLimitImpulse;
};
struct Distance {
pod::Vector3f localAnchorA;
pod::Vector3f localAnchorB;
float accumulatedImpulse;
float targetDistance;
bool isRope; // to-do: rename to something more generic
};
struct Weld {
pod::Vector3f localAnchorA;
pod::Vector3f localAnchorB;
pod::Vector3f accumulatedLinearImpulse;
pod::Vector3f accumulatedAngularImpulse;
pod::Vector3f localAxisA;
pod::Vector3f localAxisB;
pod::Vector3f localReferenceAxisA;
pod::Vector3f localReferenceAxisB;
};
struct Spring {
pod::Vector3f localAnchorA;
pod::Vector3f localAnchorB;
float accumulatedImpulse;
float restLength;
float stiffness;
float damping;
};
struct Motor {
bool enabled = false;
float targetVelocity = 0.0f;
union {
float maxMotorForce;
float maxMotorTorque;
};
float accumulatedMotorImpulse = 0.0f;
};
// this /could/ mirror the above joint-based constraints
// but a lot of the code initializes contact structs as Contact{ point, normal, depth }, which would not easily align with the above structs
struct Contact {
@ -115,7 +199,23 @@ namespace pod {
pod::BallSocket ballSocket;
pod::Hinge hinge;
pod::ConeTwist coneTwist;
pod::Slider slider;
pod::Distance distance;
pod::Weld weld;
pod::Spring spring;
};
pod::Motor motor;
};
struct SolverBodyContext {
float invM = 0;
pod::Matrix3f invI = {};
};
struct JacobianRow {
pod::Vector3f rA;
pod::Vector3f rB;
pod::Vector3f n;
};
}
@ -347,6 +447,9 @@ namespace pod {
float baumgarteCorrectionSlop = 0.005f;
float maxLinearCorrection = 0.2f;
float contactCFM = 1e-3f;
float jointCFM = 1e-4f;
uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
uint32_t manifoldCacheLifetime = 0; // 0 = derive from current settings
@ -373,4 +476,14 @@ namespace pod {
pod::BVH dynamicBvh;
pod::BVH staticBvh;
};
}
namespace uf {
namespace physics {
typedef pod::Math::num_t num_t;
namespace time = uf::time; // to-do: have separate values from the physics system
extern UF_API pod::World world;
extern UF_API pod::PhysicsSettings settings;
}
}

2
engine/src/ext/vulkan/buffer.cpp
View File

@ -146,7 +146,7 @@ bool ext::vulkan::Buffer::update( const void* data, VkDeviceSize length ) const
// to-do: fix this because it's a thorn in my side when a mesh needs to update
if ( length > allocationInfo.size ) {
UF_MSG_WARNING("Buffer update of {} exceeds buffer size of {}", length, allocationInfo.size);
//UF_MSG_WARNING("Buffer update of {} exceeds buffer size of {}", length, allocationInfo.size);
auto savedUsage = usage;
auto savedMemProps = memoryProperties;

13
engine/src/utils/math/physics/constraints.cpp
View File

@ -8,10 +8,21 @@ void impl::solveConstraint( pod::Constraint& constraint, float dt ) {
} break;
case pod::ConstraintType::HINGE: {
return impl::solveHingeConstraint( constraint, dt );
} break;
case pod::ConstraintType::CONE_TWIST: {
return impl::solveConeTwistConstraint( constraint, dt );
} break;
case pod::ConstraintType::SLIDER: {
return impl::solveSliderConstraint( constraint, dt );
} break;
case pod::ConstraintType::DISTANCE: {
return impl::solveDistanceConstraint( constraint, dt );
} break;
case pod::ConstraintType::WELD: {
return impl::solveWeldConstraint( constraint, dt );
} break;
case pod::ConstraintType::SPRING: {
return impl::solveSpringConstraint( constraint, dt );
} break;
}
}

58
engine/src/utils/math/physics/constraints/ballSocket.cpp
View File

@ -1,11 +1,15 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solveBallSocketConstraint( pod::Constraint& constraint, float dt ) {
auto& joint = constraint.ballSocket;
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.ballSocket;
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
@ -13,51 +17,31 @@ void impl::solveBallSocketConstraint( pod::Constraint& constraint, float dt ) {
auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
auto worldAnchorA = tA.position + rA;
auto worldAnchorB = tB.position + rB;
pod::Matrix3f K = {};
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
for ( auto i = 0; i < 3; ++i ) {
auto rowI = pod::JacobianRow{ rA, rB, axes[i] };
for ( auto j = 0; j < 3; ++j ) {
auto rowJ = pod::JacobianRow{ rA, rB, axes[j] };
K(i, j) = impl::computeMassMatrixLine( ctxA, ctxB, rowI, rowJ );
}
auto positionError = worldAnchorB - worldAnchorA;
K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
}
auto pA = tA.position + rA;
auto pB = tB.position + rB;
auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
auto relativeVelocity = vB - vA;
auto positionError = pB - pA;
float invMassA = a.isStatic ? 0.0f : a.inverseMass;
float invMassB = b.isStatic ? 0.0f : b.inverseMass;
float sumInvMass = invMassA + invMassB;
pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
// effective mass matrix
pod::Matrix3f K = {};
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
for ( auto i = 0; i < 3; ++i ) {
for ( auto j = 0; j < 3; ++j ) {
float termLinear = (i == j) ? sumInvMass : 0.0f;
pod::Vector3f raXnj = uf::vector::cross(rA, axes[j]);
pod::Vector3f rbXnj = uf::vector::cross(rB, axes[j]);
pod::Vector3f Ia_raXnj = uf::matrix::multiply(invIa, raXnj);
pod::Vector3f Ib_rbXnj = uf::matrix::multiply(invIb, rbXnj);
pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA);
pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB);
float termAngular = uf::vector::dot(axes[i], crossA + crossB);
K(i, j) = termLinear + termAngular;
}
}
// impulse matrix
pod::Matrix3f Kinv = uf::matrix::inverse( K );
// bias
pod::Vector3f bias = positionError * (uf::physics::settings.baumgarteCorrectionPercent / dt);
pod::Vector3f rhs = -(relativeVelocity + bias);
// solve and apply
pod::Vector3f impulse = uf::matrix::multiply( Kinv, rhs );
impl::applyImpulseTo( a, b, rA, rB, impulse, joint.accumulatedImpulse );
}

101
engine/src/utils/math/physics/constraints/coneTwist.cpp
View File

@ -1,40 +1,41 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
#include <uf/engine/scene/scene.h>
void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
auto& joint = constraint.coneTwist;
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.coneTwist;
// solve linearly
impl::solveBallSocketConstraint( constraint, dt );
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
pod::Vector3f worldTwistAxisA = uf::quaternion::rotate( tA.orientation, joint.localTwistAxisA );
pod::Vector3f worldTwistAxisB = uf::quaternion::rotate( tB.orientation, joint.localTwistAxisB );
auto taA = uf::quaternion::rotate( tA.orientation, joint.localTwistAxisA );
auto taB = uf::quaternion::rotate( tB.orientation, joint.localTwistAxisB );
pod::Vector3f worldRefA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
pod::Vector3f worldRefB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
auto raA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
auto raB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
auto invIa = impl::computeWorldInverseInertia(a);
auto invIb = impl::computeWorldInverseInertia(b);
auto relAngularVel = b.angularVelocity - a.angularVelocity;
// swing
pod::Vector3f swingAxis = uf::vector::cross( worldTwistAxisA, worldTwistAxisB );
float swingLength2 = uf::vector::magnitude( swingAxis );
auto swingAxis = uf::vector::cross( taA, taB );
float swingLength2 = uf::vector::dot( swingAxis, swingAxis );
float swingAngle = 0.0f;
float swingError = 0.0f;
bool swingActive = false;
if ( swingLength2 > EPS2 ) {
swingAxis = swingAxis / std::sqrt( swingLength2 );
float dot = uf::vector::dot( worldTwistAxisA, worldTwistAxisB );
swingAngle = std::atan2( swingLength2, dot );
float dot = uf::vector::dot( taA, taB );
swingAngle = std::atan2( std::sqrt(swingLength2), dot );
if ( swingAngle > joint.swingLimit ) {
swingError = swingAngle - joint.swingLimit;
@ -45,9 +46,9 @@ void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
}
// twist
pod::Vector3f twistAxis = worldTwistAxisA;
float proj = uf::vector::dot( worldRefB, twistAxis );
pod::Vector3f refB_projected = worldRefB - (twistAxis * proj);
pod::Vector3f twistAxis = taA;
float proj = uf::vector::dot( raB, twistAxis );
pod::Vector3f refB_projected = raB - (twistAxis * proj);
float refBLength2 = uf::vector::dot(refB_projected, refB_projected);
float twistError = 0.0f;
@ -55,9 +56,9 @@ void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
if ( refBLength2 > EPS2 ) {
refB_projected = refB_projected / std::sqrt(refBLength2);
pod::Vector3f crossRef = uf::vector::cross( worldRefA, refB_projected );
pod::Vector3f crossRef = uf::vector::cross( raA, refB_projected );
float sinTheta = uf::vector::dot( crossRef, twistAxis );
float cosTheta = uf::vector::dot( worldRefA, refB_projected );
float cosTheta = uf::vector::dot( raA, refB_projected );
float twistAngle = std::atan2( sinTheta, cosTheta );
if ( twistAngle > joint.twistLimit ) {
@ -77,11 +78,9 @@ void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
pod::Matrix2f K = {};
for ( auto i = 0; i < 2; ++i ) {
for ( auto j = 0; j < 2; ++j ) {
float kVal = 0.0f;
if ( !a.isStatic ) kVal += uf::vector::dot(axes[i], uf::matrix::multiply(invIa, axes[j]));
if ( !b.isStatic ) kVal += uf::vector::dot(axes[i], uf::matrix::multiply(invIb, axes[j]));
K(i, j) = kVal;
K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, axes[i], axes[j]);
}
K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
}
pod::Matrix2f Kinv = uf::matrix::inverse( K );
@ -89,7 +88,6 @@ void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
-(uf::vector::dot(relAngularVel, swingAxis) + ((uf::physics::settings.baumgarteCorrectionPercent / dt) * swingError)),
-(uf::vector::dot(relAngularVel, twistAxis) + ((uf::physics::settings.baumgarteCorrectionPercent / dt) * twistError)),
};
pod::Vector2f impulse = uf::matrix::multiply( Kinv, rhs );
float unconstrainedSwing = joint.accumulatedAngularImpulse.x + impulse[0];
@ -105,9 +103,9 @@ void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
joint.accumulatedAngularImpulse.x = unconstrainedSwing;
joint.accumulatedAngularImpulse.y = unconstrainedTwist;
pod::Vector3f impulseVector = (swingAxis * swingDelta) + (twistAxis * twistDelta);
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply(invIa, impulseVector);
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply(invIb, impulseVector);
auto impulseVector = (swingAxis * swingDelta) + (twistAxis * twistDelta);
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulseVector );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulseVector );
return;
}
@ -115,47 +113,28 @@ void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
// swing fallback: solve 1D
if ( swingActive ) {
float invMassN = 0.0f;
if ( !a.isStatic ) invMassN += uf::vector::dot(swingAxis, uf::matrix::multiply(invIa, swingAxis));
if ( !b.isStatic ) invMassN += uf::vector::dot(swingAxis, uf::matrix::multiply(invIb, swingAxis));
float invMassN = impl::computeAngularMassMatrixLine( ctxA, ctxB, swingAxis, swingAxis );
UF_ASSERT( invMassN > EPS );
if ( invMassN > EPS ) {
float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * swingError;
float j = -(uf::vector::dot(relAngularVel, swingAxis) + bias) / invMassN;
float jOld = joint.accumulatedAngularImpulse.x;
float jNew = std::min(0.0f, jOld + j);
joint.accumulatedAngularImpulse.x = jNew;
float jDelta = jNew - jOld;
pod::Vector3f impulseVector = swingAxis * jDelta;
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply(invIa, impulseVector);
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply(invIb, impulseVector);
}
float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * swingError;
float j = -(uf::vector::dot(relAngularVel, swingAxis) + bias) / invMassN;
auto impulse = swingAxis * impl::accumulateImpulseTo( j, joint.accumulatedAngularImpulse.x, -FLT_MAX, 0.0f );
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
}
// twist fallback: solve 1D
if ( twistActive ) {
float invMassN = 0.0f;
if ( !a.isStatic ) invMassN += uf::vector::dot(twistAxis, uf::matrix::multiply(invIa, twistAxis));
if ( !b.isStatic ) invMassN += uf::vector::dot(twistAxis, uf::matrix::multiply(invIb, twistAxis));
float invMassN = impl::computeAngularMassMatrixLine( ctxA, ctxB, twistAxis, twistAxis );
UF_ASSERT( invMassN > EPS );
if ( invMassN > EPS ) {
float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * twistError;
float j = -(uf::vector::dot(relAngularVel, twistAxis) + bias) / invMassN;
float jOld = joint.accumulatedAngularImpulse.y;
float jNew = jOld + j;
if ( twistError > 0.0f ) jNew = std::min( 0.0f, jNew );
else jNew = std::max( 0.0f, jNew );
joint.accumulatedAngularImpulse.y = jNew;
float jDelta = jNew - jOld;
pod::Vector3f impulseVector = twistAxis * jDelta;
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply(invIa, impulseVector);
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply(invIb, impulseVector);
}
float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * twistError;
float j = -(uf::vector::dot(relAngularVel, twistAxis) + bias) / invMassN;
float min = (twistError > 0.0f) ? -FLT_MAX : 0.0f;
float max = (twistError > 0.0f) ? 0.0f : FLT_MAX;
auto impulse = twistAxis * impl::accumulateImpulseTo( j, joint.accumulatedAngularImpulse.y, min, max );
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
}
}

40
engine/src/utils/math/physics/constraints/distance.cpp Normal file
View File

@ -0,0 +1,40 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solveDistanceConstraint( pod::Constraint& constraint, float dt ) {
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.distance;
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
auto pA = tA.position + rA;
auto pB = tB.position + rB;
auto delta = pB - pA;
float currentDistance2 = uf::vector::magnitude( delta );
if ( currentDistance2 < EPS2 ) return;
float currentDistance = std::sqrt( currentDistance2 );
pod::Vector3f normal = delta / currentDistance;
float distanceError = currentDistance - joint.targetDistance;
auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
float relVelAlongNormal = uf::vector::dot( vB - vA, normal );
float invMassN = impl::computeEffectiveMass( a, b, rA, rB, normal );
if ( invMassN < EPS ) return;
float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * distanceError;
if ( joint.isRope && currentDistance <= joint.targetDistance ) bias = 0;
float j = -(relVelAlongNormal + bias) / invMassN;
impl::applyImpulseTo( a, b, rA, rB, normal, j, joint.accumulatedImpulse, -FLT_MAX, joint.isRope ? 0 : FLT_MAX );
}

39
engine/src/utils/math/physics/constraints/hinge.cpp
View File

@ -1,13 +1,13 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
#include <uf/engine/scene/scene.h>
void impl::solveHingeConstraint( pod::Constraint& constraint, float dt ) {
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.hinge;
auto& motor = constraint.motor;
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
@ -15,47 +15,44 @@ void impl::solveHingeConstraint( pod::Constraint& constraint, float dt ) {
// solve linearly first
impl::solveBallSocketConstraint( constraint, dt );
auto worldAxisA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
auto worldAxisB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
auto angularError = uf::vector::cross( worldAxisA, worldAxisB );
auto waA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
auto waB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
auto angularError = uf::vector::cross( waA, waB );
pod::Vector3f tangents[2];
tangents[0] = impl::computeTangent( worldAxisA );
tangents[1] = uf::vector::cross( worldAxisA, tangents[0] );
tangents[0] = impl::computeTangent( waA );
tangents[1] = uf::vector::cross( waA, tangents[0] );
auto invIa = impl::computeWorldInverseInertia(a);
auto invIb = impl::computeWorldInverseInertia(b);
auto relAngularVel = b.angularVelocity - a.angularVelocity;
// effective mass matrix
pod::Matrix2f K = {};
for ( auto i = 0; i < 2; ++i ) {
for ( auto j = 0; j < 2; ++j ) {
float kVal = 0.0f;
if ( !a.isStatic ) kVal += uf::vector::dot(tangents[i], uf::matrix::multiply(invIa, tangents[j]));
if ( !b.isStatic ) kVal += uf::vector::dot(tangents[i], uf::matrix::multiply(invIb, tangents[j]));
K(i, j) = kVal;
K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, tangents[i], tangents[j]);
}
K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
}
// impulse matrix
pod::Matrix2f Kinv = uf::matrix::inverse(K);
// bias
pod::Vector2f rhs = {};
FOR_EACH( 2, {
rhs[i] = -(uf::vector::dot(relAngularVel, tangents[i]) + (uf::physics::settings.baumgarteCorrectionPercent / dt) * uf::vector::dot(angularError, tangents[i]));
});
// solve and apply
pod::Vector2f impulse = uf::matrix::multiply(Kinv, rhs);
for ( auto i = 0; i < 2; ++i ) {
auto jDelta = impl::accumulateImpulseTo( impulse[i], joint.accumulatedAngularImpulse[i] );
auto j = tangents[i] * jDelta;
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( invIa, j );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( invIb, j );
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, j );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, j );
}
if ( motor.enabled ) {
impl::solve1DAngularMotor( a, b, waA, motor.targetVelocity, motor.maxMotorTorque, motor.accumulatedMotorImpulse, dt );
}
}

45
engine/src/utils/math/physics/constraints/motor.cpp Normal file
View File

@ -0,0 +1,45 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solve1DLinearMotor(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& axis,
float targetVelocity, float maxForce, float& accumulatedImpulse, float dt
) {
auto vA = (a.velocity + uf::vector::cross(a.angularVelocity, rA));
auto vB = (b.velocity + uf::vector::cross(b.angularVelocity, rB));
auto relVel = vB - vA;
float currentSpeed = uf::vector::dot(relVel, axis);
float invMassN = impl::computeEffectiveMass( a, b, rA, rB, axis );
UF_ASSERT( invMassN > EPS );
float j = (targetVelocity - currentSpeed) / invMassN;
float maxImpulse = maxForce * dt;
impl::applyImpulseTo(a, b, rA, rB, axis, j, accumulatedImpulse, -maxImpulse, maxImpulse);
}
void impl::solve1DAngularMotor(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& axis,
float targetVelocity, float maxTorque,
float& accumulatedImpulse, float dt
) {
auto relAngularVel = b.angularVelocity - a.angularVelocity;
float currentSpeed = uf::vector::dot(relAngularVel, axis);
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
float invMassN = impl::computeAngularMassMatrixLine( ctxA, ctxB, axis ,axis );
UF_ASSERT( invMassN > EPS );
float j = (targetVelocity - currentSpeed) / invMassN;
float maxImpulse = maxTorque * dt;
auto jDelta = impl::accumulateImpulseTo( j, accumulatedImpulse, -maxImpulse, maxImpulse );
pod::Vector3f impulse = axis * jDelta;
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
}

104
engine/src/utils/math/physics/constraints/slider.cpp Normal file
View File

@ -0,0 +1,104 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solveSliderConstraint( pod::Constraint& constraint, float dt ) {
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.slider;
auto& motor = constraint.motor;
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
auto pA = tA.position + rA;
auto pB = tB.position + rB;
auto waA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
auto waB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
auto wrA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
auto wrB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
auto relAngularVel = b.angularVelocity - a.angularVelocity;
auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
auto relVel = vB - vA;
auto angularError = uf::vector::cross(waA, waB) + uf::vector::cross(wrA, wrB);
auto positionError = pB - pA;
{
pod::Matrix3f K = {};
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
for ( auto i = 0; i < 3; ++i ) {
for ( auto j = 0; j < 3; ++j ) {
K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, axes[i], axes[j]);
}
K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
}
pod::Matrix3f Kinv = uf::matrix::inverse( K );
pod::Vector3f bias = angularError * (uf::physics::settings.baumgarteCorrectionPercent / dt);
pod::Vector3f rhs = -(relAngularVel + bias);
pod::Vector3f impulse = uf::matrix::multiply( Kinv, rhs );
joint.accumulatedAngularImpulse += impulse;
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
}
{
pod::Vector3f t1 = wrA;
pod::Vector3f t2 = uf::vector::cross( waA, wrA );
pod::Vector3f axes[2] = { t1, t2 };
pod::Matrix2f K = {};
for ( auto i = 0; i < 2; ++i ) {
auto rowI = pod::JacobianRow{ rA, rB, axes[i] };
for ( auto j = 0; j < 2; ++j ) {
auto rowJ = pod::JacobianRow{ rA, rB, axes[j] };
K(i, j) = impl::computeMassMatrixLine( ctxA, ctxB, rowI, rowJ );
}
}
pod::Matrix2f Kinv = uf::matrix::inverse( K );
pod::Vector2f rhs = {
-(uf::vector::dot(relVel, t1) + (uf::vector::dot(positionError, t1) * (uf::physics::settings.baumgarteCorrectionPercent / dt))),
-(uf::vector::dot(relVel, t2) + (uf::vector::dot(positionError, t2) * (uf::physics::settings.baumgarteCorrectionPercent / dt)))
};
pod::Vector2f impulse = uf::matrix::multiply( Kinv, rhs );
joint.accumulatedLinearImpulse += impulse;
for( int i = 0; i < 2; i++ ) {
impl::applyImpulseTo( a, b, rA, rB, axes[i], impulse[i], joint.accumulatedLinearImpulse[i] );
}
}
if ( motor.enabled ) {
impl::solve1DLinearMotor( a, b, rA, rB, waA, motor.targetVelocity, motor.maxMotorForce, motor.accumulatedMotorImpulse, dt );
}
float currentDist = uf::vector::dot( positionError, waA );
if ( currentDist < joint.lowerLimit || currentDist > joint.upperLimit ) {
float invMassN = impl::computeEffectiveMass( a, b, rA, rB, waA );
UF_ASSERT( invMassN > EPS );
float limitError = (currentDist < joint.lowerLimit) ? (currentDist - joint.lowerLimit) : (currentDist - joint.upperLimit);
float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * limitError;
float j = -(uf::vector::dot(relVel, waA) + bias) / invMassN;
float min = ( currentDist < joint.lowerLimit ) ? 0.0f : -FLT_MAX;
float max = ( currentDist < joint.lowerLimit ) ? FLT_MAX : 0.0f;
impl::applyImpulseTo( a, b, rA, rB, waA, j, joint.accumulatedLimitImpulse, min, max );
}
}

44
engine/src/utils/math/physics/constraints/spring.cpp Normal file
View File

@ -0,0 +1,44 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solveSpringConstraint( pod::Constraint& constraint, float dt ) {
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.spring;
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
auto worldAnchorA = tA.position + rA;
auto worldAnchorB = tB.position + rB;
auto delta = worldAnchorB - worldAnchorA;
float currentDistance2 = uf::vector::magnitude( delta );
if ( currentDistance2 < EPS ) return;
float currentDistance = std::sqrt( currentDistance2 );
pod::Vector3f normal = delta / currentDistance;
float distanceError = currentDistance - joint.restLength;
auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
float relVelAlongNormal = uf::vector::dot( vB - vA, normal );
float invMassN = impl::computeEffectiveMass( a, b, rA, rB, normal );
if ( invMassN < EPS ) return;
float gamma = joint.damping + (dt * joint.stiffness);
gamma = (gamma > EPS) ? (1.0f / (dt * gamma)) : 0.0f;
float beta = dt * joint.stiffness * gamma;
float bias = distanceError * (beta / dt);
float j = -(relVelAlongNormal + bias + (gamma * joint.accumulatedImpulse)) / (invMassN + gamma);
impl::applyImpulseTo( a, b, rA, rB, normal, j, joint.accumulatedImpulse );
}

47
engine/src/utils/math/physics/constraints/weld.cpp Normal file
View File

@ -0,0 +1,47 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solveWeldConstraint( pod::Constraint& constraint, float dt ) {
auto& a = *constraint.a;
auto& b = *constraint.b;
auto& joint = constraint.weld;
// solve linearly
impl::solveBallSocketConstraint( constraint, dt );
// solve angularly
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
auto waA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
auto waB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
auto wrA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
auto wrB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
auto relAngularVel = b.angularVelocity - a.angularVelocity;
auto angularError = uf::vector::cross(waA, waB) + uf::vector::cross(wrA, wrB);
pod::Matrix3f K = {};
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
for ( auto i = 0; i < 3; ++i ) {
for ( auto j = 0; j < 3; ++j ) {
K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, axes[i], axes[j]);
}
K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
}
auto bias = angularError * (uf::physics::settings.baumgarteCorrectionPercent / dt);
auto rhs = -(relAngularVel + bias);
auto impulse = uf::matrix::multiply( uf::matrix::inverse(K), rhs );
joint.accumulatedAngularImpulse += impulse;
if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
}

68
engine/src/utils/math/physics/integration.cpp
View File

@ -1,30 +1,62 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/narrowphase.h>
pod::SolverBodyContext impl::solverBodyContext( const pod::PhysicsBody& body ) {
return { body.isStatic ? 0.0f : body.inverseMass, impl::computeWorldInverseInertia( body ) };
}
float impl::computeEffectiveMass( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& row ) {
float inverseMass = a.invM + b.invM;
float angularTerm = 0.0f;
if ( a.invM > 0.0f ) {
auto crossA = uf::vector::cross(row.rA, row.n);
auto I_crossA = uf::matrix::multiply(a.invI, crossA);
angularTerm += uf::vector::dot(uf::vector::cross(I_crossA, row.rA), row.n);
}
if ( b.invM > 0.0f ) {
auto crossB = uf::vector::cross(row.rB, row.n);
auto I_crossB = uf::matrix::multiply(b.invI, crossB);
angularTerm += uf::vector::dot(uf::vector::cross(I_crossB, row.rB), row.n);
}
return inverseMass + angularTerm;
}
// to-do: convert below to just use the above
float impl::computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& n ) {
float inverseMass = 0.0f;
if ( !a.isStatic ) inverseMass += a.inverseMass;
if ( !b.isStatic ) inverseMass += b.inverseMass;
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
auto row = pod::JacobianRow{ rA, rB, n };
return impl::computeEffectiveMass( ctxA, ctxB, row );
}
float angularTermA = 0.0f;
float angularTermB = 0.0f;
float impl::computeMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& rowI, const pod::JacobianRow& rowJ ) {
float termLinear = (a.invM + b.invM) * uf::vector::dot(rowI.n, rowJ.n);
float angularTerm = 0.0f;
if ( !a.isStatic ) {
auto invIa = impl::computeWorldInverseInertia(a);
auto crossA = uf::vector::cross(rA, n);
auto I_crossA = uf::matrix::multiply(invIa, crossA);
angularTermA = uf::vector::dot(uf::vector::cross(I_crossA, rA), n);
}
if ( !b.isStatic ) {
auto invIb = impl::computeWorldInverseInertia(b);
auto crossB = uf::vector::cross(rB, n);
auto I_crossB = uf::matrix::multiply(invIb, crossB);
angularTermB = uf::vector::dot(uf::vector::cross(I_crossB, rB), n);
if ( a.invM > 0.0f ) {
pod::Vector3f raXnj = uf::vector::cross(rowJ.rA, rowJ.n);
pod::Vector3f Ia_raXnj = uf::matrix::multiply( a.invI, raXnj );
pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rowI.rA);
angularTerm += uf::vector::dot(rowI.n, crossA);
}
// to-do: assert / handle result == 0 to avoid division by zero (this probably only would happen with two static bodies colliding, which never should happen)
return inverseMass + angularTermA + angularTermB;
if ( b.invM > 0.0f ) {
pod::Vector3f rbXnj = uf::vector::cross(rowJ.rB, rowJ.n);
pod::Vector3f Ib_rbXnj = uf::matrix::multiply( b.invI, rbXnj );
pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rowI.rB);
angularTerm += uf::vector::dot(rowI.n, crossB);
}
return termLinear + angularTerm;
}
float impl::computeAngularMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::Vector3f& n_i, const pod::Vector3f& n_j ) {
float kVal = 0.0f;
if ( a.invM > 0.0f ) kVal += uf::vector::dot(n_i, uf::matrix::multiply(a.invI, n_j));
if ( b.invM > 0.0f ) kVal += uf::vector::dot(n_i, uf::matrix::multiply(b.invI, n_j));
return kVal;
}
void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {

1
engine/src/utils/math/physics/narrowphase/aabb.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>

1
engine/src/utils/math/physics/narrowphase/capsule.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>

1
engine/src/utils/math/physics/narrowphase/mesh.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>
#include <uf/utils/math/physics/broadphase/bvh.h>

1
engine/src/utils/math/physics/narrowphase/obb.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>

1
engine/src/utils/math/physics/narrowphase/plane.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>

1
engine/src/utils/math/physics/narrowphase/ray.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>
#include <uf/utils/math/physics/broadphase/bvh.h>

1
engine/src/utils/math/physics/narrowphase/sphere.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>

1
engine/src/utils/math/physics/narrowphase/triangle.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>

46
engine/src/utils/math/physics/solvers/block.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/solvers/block.h>
@ -6,43 +7,28 @@ namespace impl {
template<size_t N, typename T = float>
bool blockNxNSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
pod::Matrix<T,N> K = {};
float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
auto ctxA = pod::SolverBodyContext{ a.isStatic ? 0.0f : a.inverseMass, impl::computeWorldInverseInertia(a) };
auto ctxB = pod::SolverBodyContext{ b.isStatic ? 0.0f : b.inverseMass, impl::computeWorldInverseInertia(b) };
auto pA = impl::getPosition( a, true );
auto pB = impl::getPosition( b, true );
auto gA = uf::physics::getGravity( a );
auto gB = uf::physics::getGravity( b );
float vSlop = std::sqrt( std::max( uf::vector::magnitude( gA ), uf::vector::magnitude( gB ) ) ) * dt;
for ( auto i = 0; i < N; i++ ) {
pod::Vector3f rA_i = manifold.points[i].point - pA;
pod::Vector3f rB_i = manifold.points[i].point - pB;
pod::Vector3f n_i = manifold.points[i].normal;
auto& pI = manifold.points[i];
auto rowI = pod::JacobianRow{ pI.point - pA, pI.point - pB, pI.normal };
for ( auto j = 0; j < N; j++ ) {
pod::Vector3f rA_j = manifold.points[j].point - pA;
pod::Vector3f rB_j = manifold.points[j].point - pB;
pod::Vector3f n_j = manifold.points[j].normal;
float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j);
pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j);
pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j);
pod::Vector3f Ia_raXnj = uf::matrix::multiply( invIa, raXnj );
pod::Vector3f Ib_rbXnj = uf::matrix::multiply( invIb, rbXnj );
pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i);
pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i);
float termAngular = uf::vector::dot(n_i, crossA + crossB);
K(i,j) = termLinear + termAngular;
auto& pJ = manifold.points[j];
auto rowJ = pod::JacobianRow{ pJ.point - pA, pJ.point - pB, pJ.normal };
K(i,j) = impl::computeMassMatrixLine( ctxA, ctxB, rowI, rowJ );
}
K(i,i) += 1e-3f;
K(i,i) += uf::physics::settings.contactCFM * ( 1.0f + ctxA.invM + ctxB.invM );
}
pod::Vector<T,N> rhs = {};
@ -56,7 +42,7 @@ namespace impl {
float vRel = uf::vector::dot((vB - vA), contact.normal);
float e = std::min(a.material.restitution, b.material.restitution);
float restitutionBias = (vRel < -1.0f) ? -e * vRel : 0.0f;
float restitutionBias = (vRel < -vSlop) ? -e * vRel : 0.0f;
rhs[i] = -vRel + restitutionBias;
K_lambda[i] = contact.accumulatedNormalImpulse;

8
engine/src/utils/math/physics/solvers/iterativeImpulse.cpp
View File

@ -1,3 +1,4 @@
#include <uf/utils/math/physics/impl.h>
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/solvers/iterativeImpulse.h>
@ -8,16 +9,21 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
// normal impulse
{
pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
pod::Vector3f rv = vB - vA;
auto gA = uf::physics::getGravity( a );
auto gB = uf::physics::getGravity( b );
float vSlop = std::sqrt( std::max( uf::vector::magnitude( gA ), uf::vector::magnitude( gB ) ) ) * dt;
float restitutionBias = 0.0f;
float e = std::min(a.material.restitution, b.material.restitution);
float velAlongNormal = uf::vector::dot(rv, contact.normal);
if ( velAlongNormal < -1.0f ) restitutionBias = -e * velAlongNormal;
if ( velAlongNormal < -vSlop ) restitutionBias = -e * velAlongNormal;
float targetVelocity = restitutionBias;
float jN = (targetVelocity - velAlongNormal) / invMassN;

35
engine/src/utils/math/physics/solvers/ngs.cpp
View File

@ -15,30 +15,33 @@ void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt,
if ( a.isStatic && b.isStatic ) continue;
for ( auto& c : manifold.points ) {
pod::Vector3f rA = uf::quaternion::rotate( tA.orientation, c.localA );
pod::Vector3f rB = uf::quaternion::rotate( tB.orientation, c.localB );
pod::Vector3f worldA = tA.position + rA;
pod::Vector3f worldB = tB.position + rB;
auto ctxA = impl::solverBodyContext( a );
auto ctxB = impl::solverBodyContext( b );
float penetration = uf::vector::dot( worldB - worldA, c.normal );
auto rA = uf::quaternion::rotate( tA.orientation, c.localA );
auto rB = uf::quaternion::rotate( tB.orientation, c.localB );
auto pA = tA.position + rA;
auto pB = tB.position + rB;
auto row = pod::JacobianRow{ rA, rB, c.normal };
float penetration = uf::vector::dot( pB - pA, c.normal );
float C = std::clamp( penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f, uf::physics::settings.maxLinearCorrection );
if ( C <= 0.0f ) continue;
float invMassN = impl::computeEffectiveMass( a, b, rA, rB, c.normal );
float invMassN = impl::computeEffectiveMass( ctxA, ctxB, row );
float lambda = (C / invMassN) * uf::physics::settings.baumgarteCorrectionPercent;
pod::Vector3f P = c.normal * lambda;
// apply impulses directly
if ( !a.isStatic ) {
pod::Vector3f translation = P * a.inverseMass;
if ( ctxA.invM > 0.0f ) {
pod::Vector3f translation = P * ctxA.invM;
a.transform->position -= translation;
tA.position -= translation;
pod::Matrix3f invIa = impl::computeWorldInverseInertia(a);
pod::Vector3f deltaAngleA = uf::matrix::multiply(invIa, uf::vector::cross(rA, -P));
pod::Vector3f deltaAngleA = uf::matrix::multiply(ctxA.invI, uf::vector::cross(rA, -P));
float angleA2 = uf::vector::magnitude( deltaAngleA );
if ( angleA2 > EPS2 ) {
float angleA = std::sqrt( angleA2 );
@ -48,14 +51,12 @@ void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt,
}
}
if ( !b.isStatic ) {
pod::Vector3f translation = P * b.inverseMass;
if ( ctxB.invM > 0.0f ) {
pod::Vector3f translation = P * ctxB.invM;
b.transform->position += translation;
tB.position += translation;
pod::Matrix3f invIb = impl::computeWorldInverseInertia(b);
pod::Vector3f deltaAngleB = uf::matrix::multiply(invIb, uf::vector::cross(rB, P));
pod::Vector3f deltaAngleB = uf::matrix::multiply(ctxB.invI, uf::vector::cross(rB, P));
float angleB2 = uf::vector::magnitude( deltaAngleB );
if ( angleB2 > EPS2 ) {
float angleB = std::sqrt( angleB2 );

174
engine/src/utils/math/physics/tests.inl
View File

@ -1445,4 +1445,178 @@ TEST(ConeTwist_Constraint, {
EXPECT_LE( std::fabs(twistAngle), coneTwist.twistLimit + 0.05f );
}
})
TEST(DistanceJoint_Constraint, {
pod::World world;
uf::Object objA, objB;
auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
bodyA.transform->position = {0, 10, 0};
bodyB.transform->position = {0, 5, 0};
pod::Constraint constraint;
constraint.type = pod::ConstraintType::DISTANCE;
constraint.a = &bodyA;
constraint.b = &bodyB;
constraint.distance.localAnchorA = {0, 0, 0};
constraint.distance.localAnchorB = {0, 0, 0};
constraint.distance.targetDistance = 5.0f;
constraint.distance.isRope = false;
constraint.distance.accumulatedImpulse = 0.0f;
float dt = 1.0f / 60.0f;
for (int i = 0; i < 60; ++i) {
bodyB.velocity.y -= 9.8f * dt;
for (int solverIters = 0; solverIters < 10; ++solverIters) {
impl::solveDistanceConstraint( constraint, dt );
}
bodyB.transform->position += bodyB.velocity * dt;
}
auto tA = impl::getTransform(bodyA);
auto tB = impl::getTransform(bodyB);
pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.distance.localAnchorA);
pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.distance.localAnchorB);
float currentDistance = std::sqrt(uf::vector::distanceSquared(worldAnchorA, worldAnchorB));
float error = std::abs(currentDistance - constraint.distance.targetDistance);
EXPECT_LT( error, 0.1f );
})
TEST(WeldJoint_Constraint, {
pod::World world;
uf::Object objA, objB;
auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
bodyA.transform->position = {0, 0, 0};
bodyB.transform->position = {2, 0, 0};
pod::Constraint constraint;
constraint.type = pod::ConstraintType::WELD;
constraint.a = &bodyA;
constraint.b = &bodyB;
constraint.weld.localAnchorA = {1, 0, 0};
constraint.weld.localAnchorB = {-1, 0, 0};
constraint.weld.localAxisA = {1, 0, 0};
constraint.weld.localAxisB = {1, 0, 0};
constraint.weld.localReferenceAxisA = {0, 1, 0};
constraint.weld.localReferenceAxisB = {0, 1, 0};
float dt = 1.0f / 60.0f;
for (int i = 0; i < 30; ++i) {
bodyB.velocity.y -= 10.0f * dt;
bodyB.angularVelocity.z += 5.0f * dt;
for (int solverIters = 0; solverIters < 10; ++solverIters) {
impl::solveWeldConstraint( constraint, dt );
}
bodyB.transform->position += bodyB.velocity * dt;
}
auto tA = impl::getTransform(bodyA);
auto tB = impl::getTransform(bodyB);
pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.weld.localAnchorA);
pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.weld.localAnchorB);
float posErrorSq = uf::vector::distanceSquared(worldAnchorA, worldAnchorB);
EXPECT_LT( posErrorSq, 0.1f );
})
TEST(SliderJoint_Constraint, {
pod::World world;
uf::Object objA, objB;
auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
bodyA.transform->position = {0, 0, 0};
bodyB.transform->position = {2, 0, 0};
pod::Constraint constraint;
constraint.type = pod::ConstraintType::SLIDER;
constraint.a = &bodyA;
constraint.b = &bodyB;
constraint.slider.localAnchorA = {0, 0, 0};
constraint.slider.localAnchorB = {0, 0, 0};
constraint.slider.localAxisA = {1, 0, 0};
constraint.slider.localAxisB = {1, 0, 0};
constraint.slider.localReferenceAxisA = {0, 1, 0};
constraint.slider.localReferenceAxisB = {0, 1, 0};
constraint.slider.lowerLimit = -5.0f;
constraint.slider.upperLimit = 5.0f;
float dt = 1.0f / 60.0f;
for (int i = 0; i < 60; ++i) {
bodyB.velocity.x += 50.0f * dt;
for (int solverIters = 0; solverIters < 10; ++solverIters) {
impl::solveSliderConstraint( constraint, dt );
}
bodyB.transform->position += bodyB.velocity * dt;
}
auto tA = impl::getTransform(bodyA);
auto tB = impl::getTransform(bodyB);
pod::Vector3f relPos = tB.position - tA.position;
float slideDistance = uf::vector::dot(relPos, constraint.slider.localAxisA);
EXPECT_LT( slideDistance, constraint.slider.upperLimit + 0.2f );
})
TEST(SpringJoint_Constraint, {
pod::World world;
uf::Object objA, objB;
auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
bodyA.transform->position = {0, 10, 0};
bodyB.transform->position = {0, 0, 0};
pod::Constraint constraint;
constraint.type = pod::ConstraintType::SPRING;
constraint.a = &bodyA;
constraint.b = &bodyB;
constraint.spring.localAnchorA = {0, 0, 0};
constraint.spring.localAnchorB = {0, 0, 0};
constraint.spring.restLength = 5.0f;
constraint.spring.stiffness = 50.0f;
constraint.spring.damping = 5.0f;
constraint.spring.accumulatedImpulse = 0.0f;
float dt = 1.0f / 60.0f;
for (int i = 0; i < 180; ++i) {
bodyB.velocity.y -= 9.8f * dt;
for (int solverIters = 0; solverIters < 10; ++solverIters) {
impl::solveSpringConstraint( constraint, dt );
}
bodyB.transform->position += bodyB.velocity * dt;
}
auto tA = impl::getTransform(bodyA);
auto tB = impl::getTransform(bodyB);
float distance = std::sqrt(uf::vector::distanceSquared(tA.position, tB.position));
EXPECT_LT( distance, constraint.spring.restLength + 2.0f );
EXPECT_GT( distance, constraint.spring.restLength - 0.1f );
})