even more inane cleanup, and even more constraints
This commit is contained in:
parent
7758389c6c
commit
40157df205
@ -350,7 +350,7 @@
|
||||
"max": 0.01 // 0.2
|
||||
},
|
||||
"debug draw": {
|
||||
"dynamic": true
|
||||
"dynamic": false
|
||||
},
|
||||
"fixed step": true,
|
||||
"substeps": 4
|
||||
|
||||
@ -14,7 +14,7 @@
|
||||
"metadata": {
|
||||
"holdable": true,
|
||||
"physics": {
|
||||
"mass": 10,
|
||||
"mass": 1,
|
||||
"type": "sphere",
|
||||
"radius": 1,
|
||||
"min": [ -0.5, -0.5, -0.5 ],
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
"metadata": {
|
||||
"holdable": true,
|
||||
"physics": {
|
||||
"mass": 100,
|
||||
"mass": 1900,
|
||||
// "inertia": false,
|
||||
"type": "obb"
|
||||
// "type": "mesh"
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
"holdable": true,
|
||||
"physics": {
|
||||
"mass": 0,
|
||||
"inertia": false,
|
||||
// "inertia": false,
|
||||
// "type": "bounding box"
|
||||
"type": "mesh"
|
||||
// "type": "hull"
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "impl.h"
|
||||
#include "structs.h"
|
||||
|
||||
#include "broadphase/bvh.h"
|
||||
#include "broadphase/island.h"
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "impl.h"
|
||||
#include "structs.h"
|
||||
#include "draw.h"
|
||||
|
||||
// to-do: organize this mess
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "../impl.h"
|
||||
#include "../structs.h"
|
||||
|
||||
namespace impl {
|
||||
void solveBallSocketConstraint( pod::Constraint& constraint, float dt );
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "../impl.h"
|
||||
#include "../structs.h"
|
||||
|
||||
namespace impl {
|
||||
void solveConeTwistConstraint( pod::Constraint& constraint, float dt );
|
||||
|
||||
@ -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
17
engine/inc/uf/utils/math/physics/constraints/distance.h
Normal 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& );
|
||||
}
|
||||
}
|
||||
*/
|
||||
@ -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
17
engine/inc/uf/utils/math/physics/constraints/motor.h
Normal 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
17
engine/inc/uf/utils/math/physics/constraints/slider.h
Normal 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
17
engine/inc/uf/utils/math/physics/constraints/spring.h
Normal 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
17
engine/inc/uf/utils/math/physics/constraints/weld.h
Normal 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& );
|
||||
}
|
||||
}
|
||||
*/
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "impl.h"
|
||||
#include "structs.h"
|
||||
|
||||
namespace impl {
|
||||
struct Vertex {
|
||||
|
||||
@ -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& );
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "impl.h"
|
||||
#include "structs.h"
|
||||
|
||||
#include "narrowphase/aabb.h"
|
||||
#include "narrowphase/obb.h"
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "impl.h"
|
||||
#include "structs.h"
|
||||
|
||||
#include "constraints/contact.h"
|
||||
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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
|
||||
|
||||
@ -374,3 +477,13 @@ namespace pod {
|
||||
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;
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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 );
|
||||
}
|
||||
|
||||
@ -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
40
engine/src/utils/math/physics/constraints/distance.cpp
Normal 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 );
|
||||
}
|
||||
@ -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] );
|
||||
|
||||
auto invIa = impl::computeWorldInverseInertia(a);
|
||||
auto invIb = impl::computeWorldInverseInertia(b);
|
||||
tangents[0] = impl::computeTangent( waA );
|
||||
tangents[1] = uf::vector::cross( waA, tangents[0] );
|
||||
|
||||
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
45
engine/src/utils/math/physics/constraints/motor.cpp
Normal 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
104
engine/src/utils/math/physics/constraints/slider.cpp
Normal 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
44
engine/src/utils/math/physics/constraints/spring.cpp
Normal 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
47
engine/src/utils/math/physics/constraints/weld.cpp
Normal 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 );
|
||||
}
|
||||
@ -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,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,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,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,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,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,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,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,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,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>
|
||||
@ -7,42 +8,27 @@ namespace impl {
|
||||
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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -1446,3 +1446,177 @@ 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 );
|
||||
})
|
||||
Loading…
Reference in New Issue
Block a user