lots of physics cleanup and fixes (split-impulse performs better than a NGS position correction somehow?)
This commit is contained in:
parent
098281ad81
commit
9097aebc92
@ -337,10 +337,11 @@
|
||||
},
|
||||
"physics": {
|
||||
"warmup solver": true,
|
||||
"block solver": true,
|
||||
"resolve block solver": true,
|
||||
"block solver": false,
|
||||
"resolve block true": true,
|
||||
"correction percent": 0.2,
|
||||
"correction slop": 0.005,
|
||||
"correction slop": 0.01,
|
||||
"max correction": 0.01,
|
||||
"gjk": false,
|
||||
"debug draw": {
|
||||
"dynamic": true
|
||||
|
||||
@ -6,9 +6,9 @@
|
||||
"metadata": {
|
||||
"holdable": true,
|
||||
"physics": {
|
||||
"mass": 1,
|
||||
"mass": 100,
|
||||
// "inertia": false,
|
||||
"type": "bounding box"
|
||||
"type": "obb"
|
||||
// "type": "mesh"
|
||||
// "type": "hull"
|
||||
}
|
||||
|
||||
@ -38,7 +38,8 @@ namespace impl {
|
||||
pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri );
|
||||
pod::Vector3f orientNormalToAB( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Vector3f n );
|
||||
float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri );
|
||||
bool testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis );
|
||||
bool testSeparatingAxis( const pod::Triangle& triangle, const pod::OBB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis );
|
||||
bool testSeparatingAxis( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f axesA[3], const pod::Vector3f axesB[3], const pod::Vector3f& axis, float& outMinOverlap, pod::Vector3f& outBestAxis );
|
||||
void clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane );
|
||||
void clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::AABB& plane );
|
||||
pod::Vector3f triangleCenter( const pod::Triangle& tri );
|
||||
@ -63,6 +64,14 @@ namespace impl {
|
||||
/*FORCE_INLINE*/ pod::AABB mergeAabb( const pod::AABB& a, const pod::AABB& b );
|
||||
/*FORCE_INLINE*/ pod::Vector3f aabbCenter( const pod::AABB& aabb );
|
||||
/*FORCE_INLINE*/ pod::Vector3f aabbExtent( const pod::AABB& aabb );
|
||||
/*FORCE_INLINE*/ pod::Vector3f obbMin( const pod::OBB& obb );
|
||||
/*FORCE_INLINE*/ pod::Vector3f obbMax( const pod::OBB& obb );
|
||||
/*FORCE_INLINE*/ pod::OBB aabbToObb( const pod::AABB& aabb );
|
||||
/*FORCE_INLINE*/ pod::AABB obbToAabb( const pod::OBB& obb );
|
||||
/*FORCE_INLINE*/ void boxAxes( pod::Vector3f axes[3] );
|
||||
/*FORCE_INLINE*/ void boxAxes( pod::Vector3f axes[3], const pod::Transform<>& transform );
|
||||
/*FORCE_INLINE*/ pod::Vector3f extentFromAxes( const pod::OBB& box, const pod::Vector3f axes[3] );
|
||||
/*FORCE_INLINE*/ float projectExtents( const pod::OBB& box, const pod::Vector3f& normal, const pod::Vector3f axes[3] );
|
||||
pod::AABB transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform );
|
||||
std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body );
|
||||
pod::AABB computeAABB( const pod::PhysicsBody& body );
|
||||
|
||||
@ -289,6 +289,7 @@ namespace pod {
|
||||
uint32_t solverIterations = 10;
|
||||
float baumgarteCorrectionPercent = 0.2f;
|
||||
float baumgarteCorrectionSlop = 0.005f;
|
||||
float maxLinearCorrection = 0.2f;
|
||||
|
||||
uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
|
||||
uint32_t manifoldCacheLifetime = 0; // 0 = derive from current settings
|
||||
@ -371,6 +372,7 @@ namespace uf {
|
||||
|
||||
pod::PhysicsBody& UF_API create( uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
@ -378,6 +380,7 @@ namespace uf {
|
||||
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} );
|
||||
|
||||
@ -8,5 +8,5 @@
|
||||
namespace impl {
|
||||
void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
|
||||
void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
|
||||
void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 2 );
|
||||
void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 4 );
|
||||
}
|
||||
@ -12,9 +12,14 @@ namespace pod {
|
||||
struct AABB {
|
||||
alignas(16) pod::Vector3f min;
|
||||
alignas(16) pod::Vector3f max;
|
||||
// operator OBB() const { return OBB{ (min + max) * 0.5f, (min - max) * 0.5f }; }
|
||||
};
|
||||
|
||||
typedef AABB OBB;
|
||||
struct OBB {
|
||||
alignas(16) pod::Vector3f center;
|
||||
alignas(16) pod::Vector3f extent;
|
||||
// operator AABB() const { return AABB{ center - extent, center + extent }; }
|
||||
};
|
||||
|
||||
struct Sphere {
|
||||
float radius;
|
||||
|
||||
@ -212,6 +212,7 @@ void UF_API uf::load( ext::json::Value& json ) {
|
||||
uf::physics::settings.solverIterations = configEnginePhysicsJson["solver iterations"].as(uf::physics::settings.solverIterations);
|
||||
uf::physics::settings.baumgarteCorrectionPercent = configEnginePhysicsJson["correction percent"].as(uf::physics::settings.baumgarteCorrectionPercent);
|
||||
uf::physics::settings.baumgarteCorrectionSlop = configEnginePhysicsJson["correction slop"].as(uf::physics::settings.baumgarteCorrectionSlop);
|
||||
uf::physics::settings.maxLinearCorrection = configEnginePhysicsJson["max correction"].as(uf::physics::settings.maxLinearCorrection);
|
||||
|
||||
auto& configEnginePhysicsDebugDrawJson = configEnginePhysicsJson["debug draw"];
|
||||
if ( ext::json::isObject( configEnginePhysicsDebugDrawJson ) ) {
|
||||
|
||||
@ -1321,10 +1321,10 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
|
||||
auto max = bounds.max; // uf::matrix::multiply<float>( model, bounds.max, 1.0f );
|
||||
|
||||
pod::Vector3f center = (max + min) * 0.5f;
|
||||
pod::Vector3f corner = uf::vector::abs(max - min) * 0.5f;
|
||||
pod::Vector3f extent = uf::vector::abs(max - min) * 0.5f;
|
||||
|
||||
if ( ext::json::isNull( metadataJson["physics"]["center"] ) ) metadataJson["physics"]["center"] = uf::vector::encode( center );
|
||||
if ( ext::json::isNull( metadataJson["physics"]["corner"] ) ) metadataJson["physics"]["corner"] = uf::vector::encode( corner );
|
||||
if ( ext::json::isNull( metadataJson["physics"]["extent"] ) ) metadataJson["physics"]["extent"] = uf::vector::encode( extent );
|
||||
if ( ext::json::isNull( metadataJson["physics"]["min"] ) ) metadataJson["physics"]["min"] = uf::vector::encode( min );
|
||||
if ( ext::json::isNull( metadataJson["physics"]["max"] ) ) metadataJson["physics"]["max"] = uf::vector::encode( max );
|
||||
}
|
||||
|
||||
@ -137,13 +137,22 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) {
|
||||
bool recenter = metadataJsonPhysics["recenter"].as<bool>();
|
||||
pod::Vector3f offset = uf::vector::decode( metadataJsonPhysics["offset"], pod::Vector3f{} );
|
||||
|
||||
if ( offset == pod::Vector3f{} ) recenter = true;
|
||||
// if ( offset == pod::Vector3f{} ) recenter = true;
|
||||
|
||||
if ( type == "bounding box" || type == "aabb" ) {
|
||||
if ( type == "bounding box" || type == "box" || type == "obb" ) {
|
||||
pod::Vector3f center = uf::vector::decode( metadataJsonPhysics["center"], pod::Vector3f{0.0f, 0.0f, 0.0f} );
|
||||
pod::Vector3f extent = uf::vector::decode( metadataJsonPhysics["extent"], pod::Vector3f{0.5f, 0.5f, 0.5f} );
|
||||
|
||||
if ( recenter ) {
|
||||
offset = center;
|
||||
center = {};
|
||||
}
|
||||
|
||||
uf::physics::create( self, pod::OBB{ .center = center, .extent = extent }, mass, offset );
|
||||
} else if ( type == "aabb" ) {
|
||||
pod::Vector3f min = uf::vector::decode( metadataJsonPhysics["min"], pod::Vector3f{-0.5f, -0.5f, -0.5f} );
|
||||
pod::Vector3f max = uf::vector::decode( metadataJsonPhysics["max"], pod::Vector3f{0.5f, 0.5f, 0.5f} );
|
||||
|
||||
// recenter
|
||||
if ( recenter ) {
|
||||
pod::Vector3f center = (max + min) * 0.5f;
|
||||
pod::Vector3f extents = (max - min) * 0.5f;
|
||||
|
||||
@ -382,7 +382,7 @@ float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vecto
|
||||
}
|
||||
|
||||
// Separating Axis Theorem test
|
||||
bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
|
||||
bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::OBB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
|
||||
float mag = uf::vector::magnitude(axis);
|
||||
if ( mag < EPS2 ) return true;
|
||||
pod::Vector3f n = axis / mag;
|
||||
@ -395,12 +395,8 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& b
|
||||
float maxT = std::max( { p0, p1, p2 } );
|
||||
|
||||
// project box
|
||||
pod::Vector3f cB = impl::aabbCenter( box );
|
||||
pod::Vector3f eB = impl::aabbExtent( box );
|
||||
float pB = uf::vector::dot(cB, n);
|
||||
float rB = eB.x * std::fabs(uf::vector::dot(axes[0], n)) +
|
||||
eB.y * std::fabs(uf::vector::dot(axes[1], n)) +
|
||||
eB.z * std::fabs(uf::vector::dot(axes[2], n));
|
||||
float pB = uf::vector::dot( box.center, n );
|
||||
float rB = impl::projectExtents( box, n, axes );
|
||||
|
||||
float minB = pB - rB;
|
||||
float maxB = pB + rB;
|
||||
@ -416,6 +412,28 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& b
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool impl::testSeparatingAxis( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f axesA[3], const pod::Vector3f axesB[3], const pod::Vector3f& axis, float& outMinOverlap, pod::Vector3f& outBestAxis ) {
|
||||
float mag = uf::vector::magnitude(axis);
|
||||
if ( mag < EPS2 ) return true;
|
||||
pod::Vector3f n = axis / mag;
|
||||
|
||||
float pA = uf::vector::dot( boxA.center, n );
|
||||
float rA = impl::projectExtents( boxA, n, axesA );
|
||||
|
||||
float pB = uf::vector::dot( boxB.center, n );
|
||||
float rB = impl::projectExtents( boxB, n, axesB );
|
||||
|
||||
float dist = std::fabs( pB - pA );
|
||||
float overlap = ( rA + rB ) - dist;
|
||||
|
||||
if ( overlap < 0.0f ) return false;
|
||||
|
||||
if ( overlap < outMinOverlap ) {
|
||||
outMinOverlap = overlap;
|
||||
outBestAxis = n;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Sutherland-Hodgman polygon clipping
|
||||
void impl::clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane ) {
|
||||
@ -649,29 +667,63 @@ pod::Vector3f impl::aabbCenter( const pod::AABB& aabb ) {
|
||||
pod::Vector3f impl::aabbExtent( const pod::AABB& aabb ) {
|
||||
return ( aabb.max - aabb.min ) * 0.5f;
|
||||
}
|
||||
// returns the min bound of an OBB
|
||||
pod::Vector3f impl::obbMin( const pod::OBB& obb ) {
|
||||
return obb.center - obb.extent;
|
||||
}
|
||||
// returns the max bound of an OBB
|
||||
pod::Vector3f impl::obbMax( const pod::OBB& obb ) {
|
||||
return obb.center + obb.extent;
|
||||
}
|
||||
// converts a min-max AABB to center-extents OBB
|
||||
pod::OBB impl::aabbToObb( const pod::AABB& aabb ) {
|
||||
return pod::OBB{
|
||||
.center = impl::aabbCenter( aabb ),
|
||||
.extent = impl::aabbExtent( aabb ),
|
||||
};
|
||||
}
|
||||
// converts a center-extents OBB to min-max AABB
|
||||
pod::AABB impl::obbToAabb( const pod::OBB& obb ) {
|
||||
return pod::AABB{
|
||||
.min = impl::obbMin( obb ),
|
||||
.max = impl::obbMax( obb ),
|
||||
};
|
||||
}
|
||||
// returns AABB axes
|
||||
void impl::boxAxes( pod::Vector3f axes[3] ) {
|
||||
axes[0] = {1,0,0};
|
||||
axes[1] = {0,1,0};
|
||||
axes[2] = {0,0,1};
|
||||
}
|
||||
// returns OBB axes
|
||||
void impl::boxAxes( pod::Vector3f axes[3], const pod::Transform<>& transform ) {
|
||||
axes[0] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{1,0,0});
|
||||
axes[1] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{0,1,0});
|
||||
axes[2] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{0,0,1});
|
||||
}
|
||||
// computes a box's extents from given axes
|
||||
pod::Vector3f impl::extentFromAxes( const pod::OBB& box, const pod::Vector3f axes[3] ) {
|
||||
return ( uf::vector::abs(axes[0]) * box.extent.x ) + ( uf::vector::abs(axes[1]) * box.extent.y ) + ( uf::vector::abs(axes[2]) * box.extent.z );
|
||||
}
|
||||
//
|
||||
float impl::projectExtents( const pod::OBB& box, const pod::Vector3f& normal, const pod::Vector3f axes[3] ) {
|
||||
return uf::vector::dot(box.extent, uf::vector::abs( pod::Vector3f{
|
||||
uf::vector::dot(axes[0], normal),
|
||||
uf::vector::dot(axes[1], normal),
|
||||
uf::vector::dot(axes[2], normal)
|
||||
} ) );
|
||||
// return box.extent.x * std::fabs(uf::vector::dot(axes[0], normal)) + box.extent.y * std::fabs(uf::vector::dot(axes[1], normal)) + box.extent.z * std::fabs(uf::vector::dot(axes[2], normal));
|
||||
}
|
||||
// transforms an AABB into world-space
|
||||
pod::AABB impl::transformAabbToWorld( const pod::AABB& aabb, const pod::Transform<>& transform ) {
|
||||
// to-do: flatten, since transform might not be flattened (even though getTransform does that)
|
||||
const auto& q = transform.orientation;
|
||||
const auto& p = transform.position;
|
||||
auto box = impl::aabbToObb( aabb );
|
||||
pod::Vector3f axes[3];
|
||||
impl::boxAxes( axes, transform );
|
||||
|
||||
pod::Vector3f cB = impl::aabbCenter( aabb );
|
||||
pod::Vector3f eB = impl::aabbExtent( aabb );
|
||||
pod::Vector3f axes[] = {
|
||||
uf::quaternion::rotate(q, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(q, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(q, pod::Vector3f{0,0,1}),
|
||||
};
|
||||
pod::Vector3f center = uf::quaternion::rotate(transform.orientation, box.center) + transform.position;
|
||||
pod::Vector3f extent = impl::extentFromAxes( box, axes );
|
||||
|
||||
pod::Vector3f cW = uf::quaternion::rotate(q, cB) + p;
|
||||
// to-do: a cleaner way of doing this with uf::vector::abs
|
||||
pod::Vector3f cE = {
|
||||
fabs(axes[0].x) * eB.x + fabs(axes[1].x) * eB.y + fabs(axes[2].x) * eB.z,
|
||||
fabs(axes[0].y) * eB.x + fabs(axes[1].y) * eB.y + fabs(axes[2].y) * eB.z,
|
||||
fabs(axes[0].z) * eB.x + fabs(axes[1].z) * eB.y + fabs(axes[2].z) * eB.z
|
||||
};
|
||||
|
||||
return { cW - cE, cW + cE };
|
||||
return { center - extent, center + extent };
|
||||
}
|
||||
// returns the line segment of a capsule
|
||||
std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::PhysicsBody& body ) {
|
||||
@ -688,15 +740,11 @@ std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::Phys
|
||||
pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
|
||||
const auto transform = impl::getTransform( body );
|
||||
switch ( body.collider.type ) {
|
||||
case pod::ShapeType::AABB:
|
||||
case pod::ShapeType::OBB: {
|
||||
case pod::ShapeType::AABB: {
|
||||
return impl::transformAabbToWorld( body.collider.aabb, transform );
|
||||
/*
|
||||
return {
|
||||
transform.position + body.collider.aabb.min,
|
||||
transform.position + body.collider.aabb.max,
|
||||
};
|
||||
*/
|
||||
} break;
|
||||
case pod::ShapeType::OBB: {
|
||||
return impl::transformAabbToWorld( impl::obbToAabb( body.collider.obb ), transform );
|
||||
} break;
|
||||
case pod::ShapeType::SPHERE: {
|
||||
return {
|
||||
|
||||
@ -190,7 +190,7 @@ void uf::physics::step( pod::World& world, float dt ) {
|
||||
// pass manifolds to solver
|
||||
impl::solveContacts( manifolds, dt );
|
||||
// do position correction
|
||||
//impl::solvePositions( manifolds, dt );
|
||||
impl::solvePositions( manifolds, dt );
|
||||
// cache manifold positions
|
||||
if ( uf::physics::settings.warmupSolver ) {
|
||||
impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
|
||||
@ -265,14 +265,20 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) {
|
||||
}
|
||||
|
||||
switch ( body.collider.type ) {
|
||||
case pod::ShapeType::AABB:
|
||||
case pod::ShapeType::OBB: {
|
||||
case pod::ShapeType::AABB: {
|
||||
pod::Vector3f dims = (body.collider.aabb.max - body.collider.aabb.min);
|
||||
pod::Vector3f dimsSq = dims * dims;
|
||||
body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
|
||||
body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } );
|
||||
body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
|
||||
} break;
|
||||
case pod::ShapeType::OBB: {
|
||||
pod::Vector3f dims = body.collider.obb.extent * 2.0f;
|
||||
pod::Vector3f dimsSq = dims * dims;
|
||||
body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
|
||||
body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } );
|
||||
body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
|
||||
} break;
|
||||
case pod::ShapeType::SPHERE: {
|
||||
float I = 0.4f * body.mass * body.collider.sphere.radius * body.collider.sphere.radius;
|
||||
float invI = 1.0f / I;
|
||||
@ -464,14 +470,22 @@ pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, fl
|
||||
}
|
||||
pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) {
|
||||
auto& body = uf::physics::create( world, object, mass, offset );
|
||||
//body.collider.type = pod::ShapeType::AABB;
|
||||
body.collider.type = pod::ShapeType::OBB;
|
||||
body.collider.type = pod::ShapeType::AABB;
|
||||
body.collider.aabb = aabb;
|
||||
body.bounds = impl::computeAABB( body );
|
||||
|
||||
uf::physics::updateInertia( body );
|
||||
return body;
|
||||
}
|
||||
pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::OBB& obb, float mass, const pod::Vector3f& offset ) {
|
||||
auto& body = uf::physics::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::OBB;
|
||||
body.collider.obb = obb;
|
||||
body.bounds = impl::computeAABB( body );
|
||||
|
||||
uf::physics::updateInertia( body );
|
||||
return body;
|
||||
}
|
||||
pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
|
||||
auto& body = uf::physics::create( world, object, mass, offset );
|
||||
body.collider.type = pod::ShapeType::SPHERE;
|
||||
@ -541,6 +555,11 @@ pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::AABB& aabb
|
||||
auto& world = scene.getComponent<pod::World>();
|
||||
return create( world, object, aabb, mass, offset );
|
||||
}
|
||||
pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::OBB& obb, float mass, const pod::Vector3f& offset ) {
|
||||
auto& scene = uf::scene::getCurrentScene();
|
||||
auto& world = scene.getComponent<pod::World>();
|
||||
return create( world, object, obb, mass, offset );
|
||||
}
|
||||
pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
|
||||
auto& scene = uf::scene::getCurrentScene();
|
||||
auto& world = scene.getComponent<pod::World>();
|
||||
|
||||
@ -23,10 +23,8 @@ float impl::computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, cons
|
||||
angularTermB = uf::vector::dot(uf::vector::cross(I_crossB, rB), n);
|
||||
}
|
||||
|
||||
float result = inverseMass + angularTermA + angularTermB;
|
||||
// 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)
|
||||
if ( result < EPS ) result = 1.0f;
|
||||
return result;
|
||||
return inverseMass + angularTermA + angularTermB;
|
||||
}
|
||||
|
||||
void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
|
||||
@ -41,7 +39,6 @@ void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::
|
||||
b.angularVelocity += uf::matrix::multiply( invIb, uf::vector::cross(rB, impulse) );
|
||||
}
|
||||
}
|
||||
|
||||
void impl::applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
|
||||
if ( !a.isStatic ) {
|
||||
a.pseudoVelocity -= impulse * a.inverseMass;
|
||||
@ -324,14 +321,12 @@ void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previo
|
||||
|
||||
validContact.accumulatedNormalImpulse *= decay;
|
||||
validContact.accumulatedTangentImpulse *= decay;
|
||||
validContact.accumulatedPseudoImpulse = 0.0f;
|
||||
|
||||
bool isDuplicate = false;
|
||||
for ( auto& c : current.points ) {
|
||||
for ( auto& c : merged ) {
|
||||
if ( impl::similarContact( validContact, c ) ) {
|
||||
c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
|
||||
c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
|
||||
c.accumulatedPseudoImpulse = validContact.accumulatedPseudoImpulse;
|
||||
c.lifetime = validContact.lifetime;
|
||||
isDuplicate = true;
|
||||
break;
|
||||
@ -474,15 +469,18 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) {
|
||||
uf::transform::rotate( *body.transform/*.reference*/, dq );
|
||||
}
|
||||
|
||||
// split impulse updates
|
||||
// pseudo-impulse position correction
|
||||
{
|
||||
body.transform->position += body.pseudoVelocity * dt;
|
||||
|
||||
float pseudoAngularSpeed2 = uf::vector::magnitude( body.pseudoAngularVelocity );
|
||||
if ( pseudoAngularSpeed2 > EPS ) {
|
||||
float pseudoAngularSpeed = std::sqrt( pseudoAngularSpeed2 );
|
||||
pod::Quaternion<> dq = uf::quaternion::axisAngle( body.pseudoAngularVelocity / pseudoAngularSpeed, pseudoAngularSpeed * dt );
|
||||
uf::transform::rotate( *body.transform/*.reference*/, dq );
|
||||
pod::Vector3f axis = body.pseudoAngularVelocity / pseudoAngularSpeed;
|
||||
|
||||
float clampedSpeed = std::min(pseudoAngularSpeed, (2.0f * M_PI / 180.0f) / dt);
|
||||
pod::Quaternion<> dq = uf::quaternion::axisAngle( axis, clampedSpeed * dt );
|
||||
uf::transform::rotate( *body.transform, dq );
|
||||
}
|
||||
|
||||
// reset
|
||||
|
||||
@ -50,13 +50,46 @@ void impl::getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& di
|
||||
outPoly[i] = hasTransform ? uf::transform::apply( transform, body.collider.triangle.points[i] ) : body.collider.triangle.points[i];
|
||||
});
|
||||
} break;
|
||||
case pod::ShapeType::AABB:
|
||||
case pod::ShapeType::AABB: {
|
||||
outCount = 4;
|
||||
pod::Vector3f n = localDir;
|
||||
pod::Vector3f absN = uf::vector::abs( n );
|
||||
pod::Vector3f min = body.collider.aabb.min;
|
||||
pod::Vector3f max = body.collider.aabb.max;
|
||||
|
||||
// dominant axis
|
||||
if ( absN.x > absN.y && absN.x > absN.z ) {
|
||||
float x = (n.x > 0) ? max.x : min.x;
|
||||
outPoly[0] = {x, min.y, max.z};
|
||||
outPoly[1] = {x, min.y, min.z};
|
||||
outPoly[2] = {x, max.y, min.z};
|
||||
outPoly[3] = {x, max.y, max.z};
|
||||
if ( n.x < 0 ) std::swap(outPoly[1], outPoly[3]);
|
||||
} else if ( absN.y > absN.z ) {
|
||||
float y = (n.y > 0) ? max.y : min.y;
|
||||
outPoly[0] = {max.x, y, max.z};
|
||||
outPoly[1] = {max.x, y, min.z};
|
||||
outPoly[2] = {min.x, y, min.z};
|
||||
outPoly[3] = {min.x, y, max.z};
|
||||
if ( n.y < 0 ) std::swap(outPoly[1], outPoly[3]);
|
||||
} else {
|
||||
float z = (n.z > 0) ? max.z : min.z;
|
||||
outPoly[0] = {min.x, max.y, z};
|
||||
outPoly[1] = {min.x, min.y, z};
|
||||
outPoly[2] = {max.x, min.y, z};
|
||||
outPoly[3] = {max.x, max.y, z};
|
||||
if (n.z < 0) std::swap(outPoly[1], outPoly[3]);
|
||||
}
|
||||
FOR_EACH(4, {
|
||||
outPoly[i] = uf::transform::apply(transform, outPoly[i]);
|
||||
});
|
||||
} break;
|
||||
case pod::ShapeType::OBB: {
|
||||
outCount = 4;
|
||||
pod::Vector3f n = localDir;
|
||||
pod::Vector3f absN = { std::fabs(n.x), std::fabs(n.y), std::fabs(n.z) };
|
||||
pod::Vector3f min = body.collider.aabb.min;
|
||||
pod::Vector3f max = body.collider.aabb.max;
|
||||
pod::Vector3f absN = uf::vector::abs( n );
|
||||
pod::Vector3f min = body.collider.obb.center - body.collider.obb.extent;
|
||||
pod::Vector3f max = body.collider.obb.center + body.collider.obb.extent;
|
||||
|
||||
// dominant axis
|
||||
if ( absN.x > absN.y && absN.x > absN.z ) {
|
||||
|
||||
@ -12,11 +12,12 @@ pod::Vector3f impl::support( const pod::PhysicsBody& body, const pod::Vector3f&
|
||||
};
|
||||
} break;
|
||||
case pod::ShapeType::OBB: {
|
||||
auto box = impl::obbToAabb( body.collider.obb );
|
||||
pod::Vector3f localDir = uf::quaternion::rotate( uf::quaternion::inverse(transform.orientation), dir );
|
||||
pod::Vector3f localPt = {
|
||||
( localDir.x >= 0.0f ) ? body.collider.obb.max.x : body.collider.obb.min.x,
|
||||
( localDir.y >= 0.0f ) ? body.collider.obb.max.y : body.collider.obb.min.y,
|
||||
( localDir.z >= 0.0f ) ? body.collider.obb.max.z : body.collider.obb.min.z
|
||||
( localDir.x >= 0.0f ) ? box.max.x : box.min.x,
|
||||
( localDir.y >= 0.0f ) ? box.max.y : box.min.y,
|
||||
( localDir.z >= 0.0f ) ? box.max.z : box.min.z
|
||||
};
|
||||
return uf::transform::apply( transform, localPt );
|
||||
} break;
|
||||
|
||||
@ -1,154 +1,182 @@
|
||||
#include <uf/utils/math/physics/common.h>
|
||||
#include <uf/utils/math/physics/narrowphase.h>
|
||||
|
||||
namespace impl {
|
||||
void getIncidentFace( const pod::OBB& obb, const pod::Vector3f* axes, const pod::Vector3f& normal, pod::Vector3f* outPoly ) {
|
||||
pod::Vector3f n = -normal;
|
||||
pod::Vector3f absN = uf::vector::abs(pod::Vector3f{
|
||||
uf::vector::dot(n, axes[0]),
|
||||
uf::vector::dot(n, axes[1]),
|
||||
uf::vector::dot(n, axes[2])
|
||||
});
|
||||
|
||||
int maxAxis = 0;
|
||||
if ( absN.y > absN.x ) maxAxis = 1;
|
||||
if ( absN.z > absN.x && absN.z > absN.y ) maxAxis = 2;
|
||||
|
||||
pod::Vector3f axis = axes[maxAxis];
|
||||
if ( uf::vector::dot(n, axis) < 0.0f ) axis = -axis;
|
||||
|
||||
pod::Vector3f center = obb.center + axis * ((maxAxis == 0) ? obb.extent.x : (maxAxis == 1) ? obb.extent.y : obb.extent.z);
|
||||
|
||||
int a1 = (maxAxis + 1) % 3;
|
||||
int a2 = (maxAxis + 2) % 3;
|
||||
|
||||
float ext1 = (a1 == 0) ? obb.extent.x : (a1 == 1) ? obb.extent.y : obb.extent.z;
|
||||
float ext2 = (a2 == 0) ? obb.extent.x : (a2 == 1) ? obb.extent.y : obb.extent.z;
|
||||
|
||||
outPoly[0] = center + axis[a1] * ext1 + axis[a2] * ext2;
|
||||
outPoly[1] = center - axis[a1] * ext1 + axis[a2] * ext2;
|
||||
outPoly[2] = center - axis[a1] * ext1 - axis[a2] * ext2;
|
||||
outPoly[3] = center + axis[a1] * ext1 - axis[a2] * ext2;
|
||||
}
|
||||
|
||||
bool boxBox( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f* axesA, const pod::Vector3f* axesB, pod::Manifold& manifold ) {
|
||||
float minOverlap = FLT_MAX;
|
||||
pod::Vector3f bestAxis;
|
||||
|
||||
for ( int i = 0; i < 3; ++i ) {
|
||||
if ( !impl::testSeparatingAxis(boxA, boxB, axesA, axesB, axesA[i], minOverlap, bestAxis) ) return false;
|
||||
if ( !impl::testSeparatingAxis(boxA, boxB, axesA, axesB, axesB[i], minOverlap, bestAxis) ) return false;
|
||||
}
|
||||
|
||||
for ( int i = 0; i < 3; ++i ) {
|
||||
for ( int j = 0; j < 3; j++ ) {
|
||||
pod::Vector3f axis = uf::vector::cross(axesA[i], axesB[j]);
|
||||
if ( !impl::testSeparatingAxis(boxA, boxB, axesA, axesB, axis, minOverlap, bestAxis) ) return false;
|
||||
}
|
||||
}
|
||||
|
||||
if ( uf::vector::dot(bestAxis, boxB.center - boxA.center) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
#if 1
|
||||
pod::Vector3f contactPoint = boxA.center + bestAxis * impl::projectExtents( boxA, bestAxis, axesA );
|
||||
manifold.points.emplace_back( pod::Contact{ contactPoint, bestAxis, minOverlap } );
|
||||
#else
|
||||
auto refBox = boxA;
|
||||
auto incBox = boxB;
|
||||
auto* refAxes = axesA;
|
||||
auto* incAxes = axesB;
|
||||
bool isARef = false;
|
||||
|
||||
float maxDot = -1.0f;
|
||||
for ( int i = 0; i < 3; i++ ) {
|
||||
float dotA = std::fabs(uf::vector::dot(axesA[i], bestAxis));
|
||||
float dotB = std::fabs(uf::vector::dot(axesB[i], bestAxis));
|
||||
if ( dotA > maxDot ) { maxDot = dotA; isARef = true; }
|
||||
if ( dotB > maxDot ) { maxDot = dotB; isARef = false; }
|
||||
}
|
||||
|
||||
if ( !isARef ) {
|
||||
refBox = boxB;
|
||||
incBox = boxA;
|
||||
refAxes = axesB;
|
||||
incAxes = axesA;
|
||||
}
|
||||
|
||||
int polyCount = 4;
|
||||
pod::Vector3f poly[8];
|
||||
pod::Vector3f incNormal = isARef ? bestAxis : -bestAxis;
|
||||
impl::getIncidentFace( incBox, incAxes, incNormal, poly );
|
||||
|
||||
int refAxisIdx = 0;
|
||||
float maxRefDot = -1.0f;
|
||||
for ( int i = 0; i < 3; i++ ) {
|
||||
float d = std::fabs(uf::vector::dot(refAxes[i], bestAxis));
|
||||
if ( d > maxRefDot ) { maxRefDot = d; refAxisIdx = i; }
|
||||
}
|
||||
|
||||
pod::Vector3f refFaceNormal = refAxes[refAxisIdx];
|
||||
if ( uf::vector::dot(refFaceNormal, isARef ? bestAxis : -bestAxis ) < 0.0f) {
|
||||
refFaceNormal = -refFaceNormal;
|
||||
}
|
||||
|
||||
int a1 = ( refAxisIdx + 1 ) % 3;
|
||||
int a2 = ( refAxisIdx + 2 ) % 3;
|
||||
float ext1 = ( a1 == 0 ) ? refBox.extent.x : ( a1 == 1 ) ? refBox.extent.y : refBox.extent.z;
|
||||
float ext2 = ( a2 == 0 ) ? refBox.extent.x : ( a2 == 1 ) ? refBox.extent.y : refBox.extent.z;
|
||||
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ refAxes[a1], ext1 + uf::vector::dot(refAxes[a1], refBox.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -refAxes[a1], ext1 - uf::vector::dot(refAxes[a1], refBox.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ refAxes[a2], ext2 + uf::vector::dot(refAxes[a2], refBox.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -refAxes[a2], ext2 - uf::vector::dot(refAxes[a2], refBox.center) });
|
||||
|
||||
if ( polyCount == 0 ) return false;
|
||||
|
||||
float refExt = (refAxisIdx == 0) ? refBox.extent.x : (refAxisIdx == 1) ? refBox.extent.y : refBox.extent.z;
|
||||
pod::Vector3f refFaceCenter = refBox.center + refFaceNormal * refExt;
|
||||
float referenceOffset = uf::vector::dot(refFaceNormal, refFaceCenter);
|
||||
|
||||
for ( auto i = 0; i < polyCount; i++ ) {
|
||||
float depth = referenceOffset - uf::vector::dot(refFaceNormal, poly[i]);
|
||||
if ( depth >= 0.0f ) {
|
||||
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, depth } );
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
return manifold.points.size() > 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool impl::obbObb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
|
||||
ASSERT_COLLIDER_TYPES( OBB, OBB );
|
||||
|
||||
auto tA = impl::getTransform( a );
|
||||
auto tB = impl::getTransform( b );
|
||||
|
||||
pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
|
||||
pod::Vector3f cB = uf::transform::apply( tB, (b.collider.obb.max + b.collider.obb.min) * 0.5f );
|
||||
pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
|
||||
pod::Vector3f eB = (b.collider.obb.max - b.collider.obb.min) * 0.5f;
|
||||
auto boxA = a.collider.obb;
|
||||
auto boxB = b.collider.obb;
|
||||
|
||||
pod::Vector3f axesA[3] = {
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
|
||||
};
|
||||
pod::Vector3f axesB[3] = {
|
||||
uf::quaternion::rotate(tB.orientation, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,0,1})
|
||||
};
|
||||
boxA.center = uf::quaternion::rotate(tA.orientation, boxA.center) + tA.position;
|
||||
boxB.center = uf::quaternion::rotate(tB.orientation, boxB.center) + tB.position;
|
||||
|
||||
float minOverlap = FLT_MAX;
|
||||
pod::Vector3f bestAxis;
|
||||
pod::Vector3f axesA[3];
|
||||
pod::Vector3f axesB[3];
|
||||
impl::boxAxes( axesA, tA );
|
||||
impl::boxAxes( axesB, tB );
|
||||
|
||||
auto testAxis = [&](const pod::Vector3f& axis) -> bool {
|
||||
float mag = uf::vector::magnitude(axis);
|
||||
if (mag < EPS) return true;
|
||||
pod::Vector3f n = axis / mag;
|
||||
|
||||
float pA = uf::vector::dot(cA, n);
|
||||
float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], n)) +
|
||||
eA.y * std::fabs(uf::vector::dot(axesA[1], n)) +
|
||||
eA.z * std::fabs(uf::vector::dot(axesA[2], n));
|
||||
|
||||
float pB = uf::vector::dot(cB, n);
|
||||
float rB = eB.x * std::fabs(uf::vector::dot(axesB[0], n)) +
|
||||
eB.y * std::fabs(uf::vector::dot(axesB[1], n)) +
|
||||
eB.z * std::fabs(uf::vector::dot(axesB[2], n));
|
||||
|
||||
float dist = std::fabs(pB - pA);
|
||||
float overlap = (rA + rB) - dist;
|
||||
|
||||
if ( overlap < 0) return false;
|
||||
|
||||
if ( overlap < minOverlap ) {
|
||||
minOverlap = overlap;
|
||||
bestAxis = n;
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesA[i]) ) return false;
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesB[i]) ) return false;
|
||||
for ( auto i = 0; i < 3; ++i ) {
|
||||
for ( auto j = 0; j < 3; j++ ) if ( !testAxis(uf::vector::cross(axesA[i], axesB[j])) ) return false;
|
||||
};
|
||||
|
||||
if ( uf::vector::dot(bestAxis, cB - cA) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
// to-do: generate contact face
|
||||
pod::Vector3f contactPoint = cA + bestAxis * (eA.x * std::fabs(uf::vector::dot(axesA[0], bestAxis)) +
|
||||
eA.y * std::fabs(uf::vector::dot(axesA[1], bestAxis)) +
|
||||
eA.z * std::fabs(uf::vector::dot(axesA[2], bestAxis)));
|
||||
|
||||
manifold.points.emplace_back( pod::Contact{ contactPoint, bestAxis, minOverlap } );
|
||||
return true;
|
||||
return impl::boxBox( boxA, boxB, axesA, axesB, manifold );
|
||||
}
|
||||
|
||||
|
||||
bool impl::obbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
|
||||
ASSERT_COLLIDER_TYPES( OBB, AABB );
|
||||
ASSERT_COLLIDER_TYPES( OBB, OBB );
|
||||
|
||||
auto tA = impl::getTransform( a );
|
||||
auto tB = impl::getTransform( b );
|
||||
|
||||
pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
|
||||
pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
|
||||
pod::Vector3f axesA[3] = {
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
|
||||
};
|
||||
auto boxA = a.collider.obb;
|
||||
auto boxB = impl::aabbToObb( b.bounds );
|
||||
boxA.center = uf::quaternion::rotate(tA.orientation, boxA.center) + tA.position;
|
||||
|
||||
pod::Vector3f cB = impl::aabbCenter( b.bounds );
|
||||
pod::Vector3f eB = impl::aabbExtent( b.bounds );
|
||||
pod::Vector3f axesB[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
|
||||
pod::Vector3f axesA[3];
|
||||
pod::Vector3f axesB[3];
|
||||
impl::boxAxes( axesA, tA );
|
||||
impl::boxAxes( axesB );
|
||||
|
||||
float minOverlap = FLT_MAX;
|
||||
pod::Vector3f bestAxis;
|
||||
|
||||
auto testAxis = [&](const pod::Vector3f& axis) -> bool {
|
||||
float mag = uf::vector::magnitude(axis);
|
||||
if ( mag < EPS ) return true;
|
||||
pod::Vector3f n = axis / mag;
|
||||
|
||||
float pA = uf::vector::dot(cA, n);
|
||||
float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], n)) +
|
||||
eA.y * std::fabs(uf::vector::dot(axesA[1], n)) +
|
||||
eA.z * std::fabs(uf::vector::dot(axesA[2], n));
|
||||
|
||||
float pB = uf::vector::dot(cB, n);
|
||||
float rB = eB.x * std::fabs(n.x) + eB.y * std::fabs(n.y) + eB.z * std::fabs(n.z);
|
||||
|
||||
float dist = std::fabs(pB - pA);
|
||||
float overlap = (rA + rB) - dist;
|
||||
if ( overlap < 0 ) return false;
|
||||
if ( overlap < minOverlap ) {
|
||||
minOverlap = overlap;
|
||||
bestAxis = n;
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesA[i]) ) return false;
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesB[i]) ) return false;
|
||||
for ( auto i = 0; i < 3; ++i ) {
|
||||
for ( auto j = 0; j < 3; j++ ) if ( !testAxis(uf::vector::cross(axesA[i], axesB[j])) ) return false;
|
||||
};
|
||||
|
||||
if ( uf::vector::dot(bestAxis, cB - cA) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
pod::Vector3f contactPoint = cA + bestAxis * (eA.x * std::fabs(uf::vector::dot(axesA[0], bestAxis)) +
|
||||
eA.y * std::fabs(uf::vector::dot(axesA[1], bestAxis)) +
|
||||
eA.z * std::fabs(uf::vector::dot(axesA[2], bestAxis)));
|
||||
|
||||
manifold.points.emplace_back( pod::Contact{ contactPoint, bestAxis, minOverlap } );
|
||||
return true;
|
||||
return impl::boxBox( boxA, boxB, axesA, axesB, manifold );
|
||||
}
|
||||
|
||||
bool impl::obbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
|
||||
ASSERT_COLLIDER_TYPES( OBB, SPHERE );
|
||||
|
||||
auto tA = impl::getTransform( a );
|
||||
auto localCenter = ( a.collider.obb.max + a.collider.obb.min ) * 0.5f;
|
||||
auto extents = ( a.collider.obb.max - a.collider.obb.min ) * 0.5f;
|
||||
auto box = a.collider.obb;
|
||||
box.center = uf::quaternion::rotate(tA.orientation, box.center) + tA.position;
|
||||
|
||||
auto sphereCenter = impl::getPosition( b );
|
||||
float radius = b.collider.sphere.radius;
|
||||
|
||||
auto localP = uf::transform::applyInverse( tA, sphereCenter ) - localCenter;
|
||||
auto closestLocal = uf::vector::clamp( localP, -extents, extents );
|
||||
auto localP = uf::transform::applyInverse( tA, sphereCenter ) - box.center;
|
||||
auto closestLocal = uf::vector::clamp( localP, -box.extent, box.extent );
|
||||
|
||||
auto deltaLocal = localP - closestLocal;
|
||||
float distSq = uf::vector::dot( deltaLocal, deltaLocal );
|
||||
|
||||
if ( distSq > radius * radius ) return false;
|
||||
|
||||
auto closestWorld = uf::transform::apply( tA, closestLocal + localCenter );
|
||||
auto closestWorld = uf::transform::apply( tA, closestLocal + box.center );
|
||||
float dist = std::sqrt( distSq );
|
||||
|
||||
pod::Vector3f normal;
|
||||
@ -160,8 +188,8 @@ bool impl::obbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod:
|
||||
float sign = 1.0f;
|
||||
|
||||
FOR_EACH(3, {
|
||||
float distToMax = extents[i] - localP[i];
|
||||
float distToMin = localP[i] - (-extents[i]);
|
||||
float distToMax = box.extent[i] - localP[i];
|
||||
float distToMin = localP[i] - (-box.extent[i]);
|
||||
if (distToMax < minDist) { minDist = distToMax; axis = i; sign = 1.0f; }
|
||||
if (distToMin < minDist) { minDist = distToMin; axis = i; sign = -1.0f; }
|
||||
});
|
||||
@ -182,28 +210,26 @@ bool impl::obbPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::
|
||||
ASSERT_COLLIDER_TYPES( OBB, PLANE );
|
||||
|
||||
auto tA = impl::getTransform( a );
|
||||
pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
|
||||
pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
|
||||
pod::Vector3f axesA[3] = {
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
|
||||
};
|
||||
auto box = a.collider.obb;
|
||||
box.center = uf::quaternion::rotate(tA.orientation, box.center) + tA.position;
|
||||
|
||||
pod::Vector3f axesA[3];
|
||||
impl::boxAxes( axesA, tA );
|
||||
|
||||
pod::Vector3f normal = b.collider.plane.normal;
|
||||
float offset = b.collider.plane.offset;
|
||||
|
||||
float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], normal)) +
|
||||
eA.y * std::fabs(uf::vector::dot(axesA[1], normal)) +
|
||||
eA.z * std::fabs(uf::vector::dot(axesA[2], normal));
|
||||
float rA = box.extent.x * std::fabs(uf::vector::dot(axesA[0], normal)) +
|
||||
box.extent.y * std::fabs(uf::vector::dot(axesA[1], normal)) +
|
||||
box.extent.z * std::fabs(uf::vector::dot(axesA[2], normal));
|
||||
|
||||
float dist = uf::vector::dot(cA, normal) - offset;
|
||||
float dist = uf::vector::dot(box.center, normal) - offset;
|
||||
if ( dist > rA ) return false; // in front of plane
|
||||
|
||||
pod::Vector3f deepestPoint = cA
|
||||
- axesA[0] * eA.x * (uf::vector::dot(axesA[0], normal) > 0 ? 1.0f : -1.0f)
|
||||
- axesA[1] * eA.y * (uf::vector::dot(axesA[1], normal) > 0 ? 1.0f : -1.0f)
|
||||
- axesA[2] * eA.z * (uf::vector::dot(axesA[2], normal) > 0 ? 1.0f : -1.0f);
|
||||
pod::Vector3f deepestPoint = box.center
|
||||
- axesA[0] * box.extent.x * (uf::vector::dot(axesA[0], normal) > 0 ? 1.0f : -1.0f)
|
||||
- axesA[1] * box.extent.y * (uf::vector::dot(axesA[1], normal) > 0 ? 1.0f : -1.0f)
|
||||
- axesA[2] * box.extent.z * (uf::vector::dot(axesA[2], normal) > 0 ? 1.0f : -1.0f);
|
||||
|
||||
float penetration = rA - dist;
|
||||
manifold.points.emplace_back( pod::Contact{ deepestPoint, normal, penetration } );
|
||||
@ -214,13 +240,11 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
|
||||
ASSERT_COLLIDER_TYPES( OBB, CAPSULE );
|
||||
|
||||
auto tA = impl::getTransform( a );
|
||||
pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
|
||||
pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
|
||||
pod::Vector3f axesA[3] = {
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
|
||||
};
|
||||
auto box = a.collider.obb;
|
||||
box.center = uf::quaternion::rotate(tA.orientation, box.center) + tA.position;
|
||||
|
||||
pod::Vector3f axesA[3];
|
||||
impl::boxAxes( axesA, tA );
|
||||
|
||||
auto [p1, p2] = impl::getCapsuleSegment( b );
|
||||
pod::Vector3f cB = (p1 + p2) * 0.5f;
|
||||
@ -236,10 +260,10 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
|
||||
if (mag < EPS) return true;
|
||||
pod::Vector3f n = axis / mag;
|
||||
|
||||
float pA = uf::vector::dot(cA, n);
|
||||
float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], n)) +
|
||||
eA.y * std::fabs(uf::vector::dot(axesA[1], n)) +
|
||||
eA.z * std::fabs(uf::vector::dot(axesA[2], n));
|
||||
float pA = uf::vector::dot(box.center, n);
|
||||
float rA = box.extent.x * std::fabs(uf::vector::dot(axesA[0], n)) +
|
||||
box.extent.y * std::fabs(uf::vector::dot(axesA[1], n)) +
|
||||
box.extent.z * std::fabs(uf::vector::dot(axesA[2], n));
|
||||
|
||||
float pB = uf::vector::dot(cB, n);
|
||||
float rB = halfHeight * std::fabs(uf::vector::dot(capAxis, n)) + radius;
|
||||
@ -247,9 +271,9 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
|
||||
float dist = std::fabs(pB - pA);
|
||||
float overlap = (rA + rB) - dist;
|
||||
|
||||
if (overlap < 0) return false;
|
||||
if ( overlap < 0 ) return false;
|
||||
|
||||
if (overlap < minOverlap) {
|
||||
if ( overlap < minOverlap ) {
|
||||
minOverlap = overlap;
|
||||
bestAxis = n;
|
||||
}
|
||||
@ -260,7 +284,7 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesA[i]) ) return false;
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(uf::vector::cross(axesA[i], capAxis)) ) return false;
|
||||
|
||||
if ( uf::vector::dot(bestAxis, cB - cA) < 0.0f ) bestAxis = -bestAxis;
|
||||
if ( uf::vector::dot(bestAxis, cB - box.center) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
pod::Vector3f contactPoint = cB - bestAxis * radius;
|
||||
|
||||
@ -277,7 +301,7 @@ bool impl::obbHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::M
|
||||
}
|
||||
|
||||
void impl::drawObb( const pod::PhysicsBody& body ) {
|
||||
auto& aabb = body.collider.aabb;
|
||||
auto aabb = impl::obbToAabb( body.collider.obb );
|
||||
pod::Vector3f corners[8] = {
|
||||
{aabb.min.x, aabb.min.y, aabb.min.z}, {aabb.max.x, aabb.min.y, aabb.min.z},
|
||||
{aabb.max.x, aabb.max.y, aabb.min.z}, {aabb.min.x, aabb.max.y, aabb.min.z},
|
||||
|
||||
@ -65,7 +65,7 @@ bool impl::rayAabb( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQ
|
||||
|
||||
auto local = rayHit.contact.point - (body.bounds.min + body.bounds.max) * 0.5f;
|
||||
auto extents = (body.bounds.max - body.bounds.min) * 0.5f;
|
||||
auto absLocal = pod::Vector3f{fabs(local.x), fabs(local.y), fabs(local.z)};
|
||||
auto absLocal = uf::vector::abs( local );
|
||||
|
||||
if ( absLocal.x > absLocal.y && absLocal.x > absLocal.z ) {
|
||||
rayHit.contact.normal = { (local.x > 0 ? 1.0f : -1.0f), 0, 0 };
|
||||
@ -84,10 +84,10 @@ bool impl::rayObb( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQu
|
||||
localRay.origin = uf::transform::applyInverse( tA, ray.origin );
|
||||
localRay.direction = uf::quaternion::rotate( uf::quaternion::inverse(tA.orientation), ray.direction );
|
||||
|
||||
pod::AABB localBox = { body.collider.obb.min, body.collider.obb.max };
|
||||
auto box = impl::obbToAabb( body.collider.obb );
|
||||
|
||||
float tMin, tMax;
|
||||
if ( !impl::rayAabbIntersect( localRay, localBox, tMin, tMax ) ) return false;
|
||||
if ( !impl::rayAabbIntersect( localRay, box, tMin, tMax ) ) return false;
|
||||
|
||||
if ( tMin < rayHit.contact.penetration ) {
|
||||
rayHit.hit = true;
|
||||
@ -97,10 +97,8 @@ bool impl::rayObb( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQu
|
||||
rayHit.contact.point = ray.origin + ray.direction * tMin;
|
||||
|
||||
auto localPoint = localRay.origin + localRay.direction * tMin;
|
||||
auto center = (localBox.max + localBox.min) * 0.5f;
|
||||
auto extents = (localBox.max - localBox.min) * 0.5f;
|
||||
auto localDelta = localPoint - center;
|
||||
auto absDelta = pod::Vector3f{ std::fabs(localDelta.x) / extents.x, std::fabs(localDelta.y) / extents.y, std::fabs(localDelta.z) / extents.z };
|
||||
auto localDelta = localPoint - body.collider.obb.center;
|
||||
auto absDelta = uf::vector::abs( localDelta / body.collider.obb.extent );
|
||||
|
||||
pod::Vector3f localNormal = {0,0,0};
|
||||
if ( absDelta.x > absDelta.y && absDelta.x > absDelta.z ) localNormal.x = localDelta.x > 0 ? 1.0f : -1.0f;
|
||||
|
||||
@ -19,6 +19,61 @@ namespace impl {
|
||||
|
||||
return triangleGeneric( tri, body, manifold );
|
||||
}
|
||||
|
||||
bool triangleBox( const pod::TriangleWithNormal& tri, const pod::OBB& box, const pod::Vector3f* axes, pod::Manifold& manifold ) {
|
||||
float minOverlap = FLT_MAX;
|
||||
pod::Vector3f bestAxis;
|
||||
|
||||
if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
|
||||
|
||||
pod::Vector3f edges[3] = {
|
||||
tri.points[1] - tri.points[0],
|
||||
tri.points[2] - tri.points[1],
|
||||
tri.points[0] - tri.points[2]
|
||||
};
|
||||
for ( int i = 0; i < 3; i++ ) {
|
||||
for ( int j = 0; j < 3; j++ ) {
|
||||
auto axis = uf::vector::cross(edges[i], axes[j]);
|
||||
if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
|
||||
}
|
||||
}
|
||||
|
||||
auto cT = impl::triangleCenter(tri);
|
||||
if ( uf::vector::dot( bestAxis, box.center - cT ) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
int polyCount = 3;
|
||||
pod::Vector3f poly[8];
|
||||
poly[0] = tri.points[0];
|
||||
poly[1] = tri.points[1];
|
||||
poly[2] = tri.points[2];
|
||||
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[0], box.extent.x + uf::vector::dot(axes[0], box.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[0], box.extent.x - uf::vector::dot(axes[0], box.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[1], box.extent.y + uf::vector::dot(axes[1], box.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[1], box.extent.y - uf::vector::dot(axes[1], box.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[2], box.extent.z + uf::vector::dot(axes[2], box.center) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[2], box.extent.z - uf::vector::dot(axes[2], box.center) });
|
||||
|
||||
if ( polyCount == 0 ) return false;
|
||||
|
||||
pod::Vector3f boxSupport = box.center;
|
||||
|
||||
boxSupport -= axes[0] * std::copysign(box.extent.x, uf::vector::dot(bestAxis, axes[0]));
|
||||
boxSupport -= axes[1] * std::copysign(box.extent.y, uf::vector::dot(bestAxis, axes[1]));
|
||||
boxSupport -= axes[2] * std::copysign(box.extent.z, uf::vector::dot(bestAxis, axes[2]));
|
||||
|
||||
float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
|
||||
|
||||
for ( auto i = 0; i < polyCount; i++ ) {
|
||||
float penetration = uf::vector::dot(bestAxis, poly[i]) - referenceOffset;
|
||||
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold ) {
|
||||
@ -96,8 +151,8 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
|
||||
auto p1 = refTri.points[(i+1)%3];
|
||||
auto edge = p1 - p0;
|
||||
|
||||
//auto edgeNormal = uf::vector::normalize( uf::vector::cross( refNormal, edge ) );
|
||||
auto edgeNormal = uf::vector::normalize( uf::vector::cross( edge, refNormal ) );
|
||||
auto edgeNormal = uf::vector::normalize( uf::vector::cross( refNormal, edge ) );
|
||||
//auto edgeNormal = uf::vector::normalize( uf::vector::cross( edge, refNormal ) );
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ edgeNormal, uf::vector::dot(edgeNormal, p0) } );
|
||||
if ( polyCount == 0 ) return false;
|
||||
}
|
||||
@ -110,8 +165,7 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
|
||||
float pointProj = uf::vector::dot(bestAxis, poly[i]);
|
||||
float penetration = isAReference ? (pointProj - refOffset) : (refOffset - pointProj);
|
||||
#else
|
||||
float dist = uf::vector::dot(poly[i], refNormal) - uf::vector::dot(refNormal, refTri.points[0]);
|
||||
float penetration = -dist;
|
||||
float penetration = uf::vector::dot(refNormal, refTri.points[0]) - uf::vector::dot(poly[i], refNormal);
|
||||
#endif
|
||||
|
||||
manifold.points.emplace_back(pod::Contact{ poly[i], bestAxis, penetration });
|
||||
@ -123,131 +177,23 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
|
||||
bool impl::triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
|
||||
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
|
||||
|
||||
const auto& box = body.bounds;
|
||||
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
|
||||
auto box = impl::aabbToObb( body.bounds );
|
||||
pod::Vector3f axes[3];
|
||||
impl::boxAxes( axes );
|
||||
|
||||
float minOverlap = FLT_MAX;
|
||||
pod::Vector3f bestAxis;
|
||||
if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
|
||||
|
||||
// test edges (3 triangle edges crossed with 3 AABB axes)
|
||||
pod::Vector3f edges[3] = {
|
||||
tri.points[1] - tri.points[0],
|
||||
tri.points[2] - tri.points[1],
|
||||
tri.points[0] - tri.points[2]
|
||||
};
|
||||
for ( int i = 0; i < 3; i++ ) {
|
||||
for ( int j = 0; j < 3; j++ ) {
|
||||
auto axis = uf::vector::cross(edges[i], axes[j]);
|
||||
if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
|
||||
}
|
||||
}
|
||||
|
||||
auto cB = impl::aabbCenter(box);
|
||||
auto eB = impl::aabbExtent(box);
|
||||
auto cT = impl::triangleCenter(tri);
|
||||
if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
int polyCount = 3;
|
||||
pod::Vector3f poly[8];
|
||||
poly[0] = tri.points[0];
|
||||
poly[1] = tri.points[1];
|
||||
poly[2] = tri.points[2];
|
||||
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ { 1, 0, 0}, eB.x + cB.x} );
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ {-1, 0, 0}, eB.x - cB.x} );
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, 1, 0}, eB.y + cB.y} );
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, -1, 0}, eB.y - cB.y} );
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, 0, 1}, eB.z + cB.z} );
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, 0, -1}, eB.z - cB.z} );
|
||||
|
||||
if ( polyCount == 0 ) return false;
|
||||
|
||||
pod::Vector3f boxSupport = cB;
|
||||
boxSupport.x -= std::copysign(eB.x, bestAxis.x);
|
||||
boxSupport.y -= std::copysign(eB.y, bestAxis.y);
|
||||
boxSupport.z -= std::copysign(eB.z, bestAxis.z);
|
||||
|
||||
float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
|
||||
|
||||
for ( auto i = 0; i < polyCount; i++ ) {
|
||||
float pointProjection = uf::vector::dot(bestAxis, poly[i]);
|
||||
float penetration = pointProjection - referenceOffset;
|
||||
|
||||
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
|
||||
}
|
||||
|
||||
return true;
|
||||
return impl::triangleBox( tri, box, axes, manifold );
|
||||
}
|
||||
bool impl::triangleObb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
|
||||
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
|
||||
|
||||
const auto& box = body.bounds;
|
||||
auto tB = impl::getTransform( body );
|
||||
pod::Vector3f axes[3] = {
|
||||
uf::quaternion::rotate(tB.orientation, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,0,1})
|
||||
};
|
||||
auto transform = impl::getTransform( body );
|
||||
auto box = body.collider.obb;
|
||||
box.center = uf::quaternion::rotate(transform.orientation, box.center) + transform.position;
|
||||
|
||||
float minOverlap = FLT_MAX;
|
||||
pod::Vector3f bestAxis;
|
||||
pod::Vector3f axes[3];
|
||||
impl::boxAxes( axes, transform );
|
||||
|
||||
if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
|
||||
if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
|
||||
|
||||
pod::Vector3f edges[3] = {
|
||||
tri.points[1] - tri.points[0],
|
||||
tri.points[2] - tri.points[1],
|
||||
tri.points[0] - tri.points[2]
|
||||
};
|
||||
for ( auto i = 0; i < 3; i++ ) {
|
||||
for ( auto j = 0; j < 3; j++ ) {
|
||||
pod::Vector3f axis = uf::vector::cross( edges[i], axes[j] );
|
||||
if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
|
||||
}
|
||||
}
|
||||
|
||||
auto cB = impl::aabbCenter(box);
|
||||
auto eB = impl::aabbExtent(box);
|
||||
auto cT = impl::triangleCenter(tri);
|
||||
if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
int polyCount = 3;
|
||||
pod::Vector3f poly[8];
|
||||
poly[0] = tri.points[0];
|
||||
poly[1] = tri.points[1];
|
||||
poly[2] = tri.points[2];
|
||||
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[0], eB.x + uf::vector::dot(axes[0], cB) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[0], eB.x - uf::vector::dot(axes[0], cB) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[1], eB.y + uf::vector::dot(axes[1], cB) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[1], eB.y - uf::vector::dot(axes[1], cB) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[2], eB.z + uf::vector::dot(axes[2], cB) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[2], eB.z - uf::vector::dot(axes[2], cB) });
|
||||
|
||||
if ( polyCount == 0 ) return false;
|
||||
|
||||
pod::Vector3f boxSupport = cB;
|
||||
boxSupport.x -= std::copysign(eB.x, bestAxis.x);
|
||||
boxSupport.y -= std::copysign(eB.y, bestAxis.y);
|
||||
boxSupport.z -= std::copysign(eB.z, bestAxis.z);
|
||||
|
||||
float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
|
||||
|
||||
for ( auto i = 0; i < polyCount; i++ ) {
|
||||
float pointProjection = uf::vector::dot(bestAxis, poly[i]);
|
||||
float penetration = pointProjection - referenceOffset;
|
||||
|
||||
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
|
||||
}
|
||||
|
||||
return true;
|
||||
return impl::triangleBox( tri, box, axes, manifold );
|
||||
}
|
||||
bool impl::triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
|
||||
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
|
||||
|
||||
@ -13,21 +13,71 @@ void impl::solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt )
|
||||
if ( uf::physics::settings.warmupSolver ) for ( auto& manifold : manifolds ) impl::warmupManifold( *manifold.a, *manifold.b, manifold, dt );
|
||||
for ( auto i = 0; i < uf::physics::settings.solverIterations; ++i ) for ( auto& manifold : manifolds ) impl::resolveManifold( *manifold.a, *manifold.b, manifold, dt );
|
||||
}
|
||||
// unused
|
||||
void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations ) {
|
||||
if ( true || uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return;
|
||||
for ( auto i = 0; i < iterations; ++i ) {
|
||||
for ( auto& m : manifolds ) {
|
||||
pod::Contact s = {};
|
||||
float weight = 0;
|
||||
for ( auto& c : m.points ) {
|
||||
float w = std::max( c.penetration, 0.0f );
|
||||
s.normal += c.normal * w;
|
||||
s.penetration = std::max(s.penetration, c.penetration);
|
||||
weight += w;
|
||||
float minSeparation = 0.0f;
|
||||
|
||||
for ( auto& manifold : manifolds ) {
|
||||
auto& a = *manifold.a;
|
||||
auto& b = *manifold.b;
|
||||
auto tA = impl::getTransform( a );
|
||||
auto tB = impl::getTransform( b );
|
||||
|
||||
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;
|
||||
|
||||
float penetration = -uf::vector::dot( worldB - worldA, c.normal );
|
||||
minSeparation = std::min( minSeparation, -penetration );
|
||||
|
||||
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 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;
|
||||
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));
|
||||
|
||||
float angleA2 = uf::vector::magnitude( deltaAngleA );
|
||||
if ( angleA2 > EPS2 ) {
|
||||
float angleA = std::sqrt( angleA2);
|
||||
pod::Quaternion<> dq = uf::quaternion::axisAngle(deltaAngleA / angleA, angleA);
|
||||
uf::transform::rotate( *a.transform, dq );
|
||||
tA.orientation = uf::quaternion::multiply(dq, tA.orientation);
|
||||
}
|
||||
}
|
||||
|
||||
if ( !b.isStatic ) {
|
||||
pod::Vector3f translation = P * b.inverseMass;
|
||||
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));
|
||||
|
||||
float angleB2 = uf::vector::magnitude( deltaAngleB );
|
||||
if ( angleB2 > EPS2 ) {
|
||||
float angleB = std::sqrt( angleB2);
|
||||
pod::Quaternion<> dq = uf::quaternion::axisAngle(deltaAngleB / angleB, angleB);
|
||||
uf::transform::rotate( *b.transform, dq );
|
||||
tB.orientation = uf::quaternion::multiply(dq, tB.orientation);
|
||||
}
|
||||
}
|
||||
}
|
||||
s.normal = weight > 0.0f ? uf::vector::normalize(s.normal) : pod::Vector3f{0,1,0};
|
||||
|
||||
impl::positionCorrection( *m.a, *m.b, s );
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -45,11 +45,8 @@ namespace impl {
|
||||
K(i,i) += 1e-3f;
|
||||
}
|
||||
|
||||
pod::Vector<T,N> rhsVel = {};
|
||||
pod::Vector<T,N> K_lambdaVel = {};
|
||||
|
||||
pod::Vector<T,N> rhsPos = {};
|
||||
pod::Vector<T,N> K_lambdaPos = {};
|
||||
pod::Vector<T,N> rhs = {};
|
||||
pod::Vector<T,N> K_lambda = {};
|
||||
|
||||
for ( auto i = 0; i < N; i++ ) {
|
||||
auto& contact = manifold.points[i];
|
||||
@ -61,32 +58,19 @@ namespace impl {
|
||||
float e = std::min(a.material.restitution, b.material.restitution);
|
||||
float restitutionBias = (vRel < -1.0f) ? -e * vRel : 0.0f;
|
||||
|
||||
rhsVel[i] = -vRel + restitutionBias;
|
||||
|
||||
pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross( a.pseudoAngularVelocity, contact.point - pA );
|
||||
pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross( b.pseudoAngularVelocity, contact.point - pB );
|
||||
float pseudoVRel = uf::vector::dot((pseudoVb - pseudoVa), contact.normal);
|
||||
|
||||
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
|
||||
|
||||
rhsPos[i] = -pseudoVRel + penetrationBias;
|
||||
|
||||
K_lambdaVel[i] = contact.accumulatedNormalImpulse;
|
||||
K_lambdaPos[i] = contact.accumulatedPseudoImpulse;
|
||||
rhs[i] = -vRel + restitutionBias;
|
||||
K_lambda[i] = contact.accumulatedNormalImpulse;
|
||||
}
|
||||
|
||||
pod::Matrix<T,N> Kinv = uf::matrix::inverse( K );
|
||||
|
||||
pod::Vector<T,N> residualVel = rhsVel - uf::matrix::multiply( K, K_lambdaVel );
|
||||
pod::Vector<T,N> dLambdaVel = uf::matrix::multiply( Kinv, residualVel );
|
||||
|
||||
pod::Vector<T,N> residualPos = rhsPos - uf::matrix::multiply( K, K_lambdaPos );
|
||||
pod::Vector<T,N> dLambdaPos = uf::matrix::multiply( Kinv, residualPos );
|
||||
pod::Vector<T,N> residual = rhs - uf::matrix::multiply( K, K_lambda );
|
||||
pod::Vector<T,N> dLambda = uf::matrix::multiply( Kinv, residual );
|
||||
|
||||
// check if contacts are all valid
|
||||
int invalidContactIndex = -1;
|
||||
for ( auto i = 0; i < N; i++ ) {
|
||||
if ( K_lambdaVel[i] + dLambdaVel[i] < 0.0f || K_lambdaPos[i] + dLambdaPos[i] < 0.0f ) {
|
||||
if ( K_lambda[i] + dLambda[i] < 0.0f ) {
|
||||
invalidContactIndex = i;
|
||||
break;
|
||||
}
|
||||
@ -96,10 +80,17 @@ namespace impl {
|
||||
bool success = false;
|
||||
// reduce the manifold
|
||||
if ( uf::physics::settings.resolveBlockContact && N > 1 ) {
|
||||
#if 1
|
||||
pod::Manifold reducedManifold = manifold;
|
||||
reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
|
||||
// re-solve
|
||||
success = impl::blockSolver( a, b, reducedManifold, dt );
|
||||
// copy back to original manifold
|
||||
if ( success ) manifold = reducedManifold;
|
||||
#else
|
||||
pod::Manifold reducedManifold = manifold;
|
||||
reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
|
||||
manifold.points[invalidContactIndex].accumulatedNormalImpulse = 0.0f;
|
||||
manifold.points[invalidContactIndex].accumulatedPseudoImpulse = 0.0f;
|
||||
manifold.points[invalidContactIndex].accumulatedTangentImpulse = 0.0f;
|
||||
|
||||
// re-solve
|
||||
@ -110,6 +101,7 @@ namespace impl {
|
||||
manifold.points[i] = reducedManifold.points[r++];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
return success;
|
||||
}
|
||||
@ -119,19 +111,31 @@ namespace impl {
|
||||
pod::Vector3f rA = manifold.points[i].point - pA;
|
||||
pod::Vector3f rB = manifold.points[i].point - pB;
|
||||
|
||||
// real impulse
|
||||
// normal impulse
|
||||
{
|
||||
float newLambdaVel = contact.accumulatedNormalImpulse + dLambdaVel[i];
|
||||
dLambdaVel[i] = newLambdaVel - contact.accumulatedNormalImpulse;
|
||||
contact.accumulatedNormalImpulse = newLambdaVel;
|
||||
impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaVel[i] );
|
||||
float newLambda = contact.accumulatedNormalImpulse + dLambda[i];
|
||||
dLambda[i] = newLambda - contact.accumulatedNormalImpulse;
|
||||
contact.accumulatedNormalImpulse = newLambda;
|
||||
impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] );
|
||||
}
|
||||
// pseudo impulse
|
||||
{
|
||||
float newLambdaPos = contact.accumulatedPseudoImpulse + dLambdaPos[i];
|
||||
dLambdaPos[i] = newLambdaPos - contact.accumulatedPseudoImpulse;
|
||||
contact.accumulatedPseudoImpulse = newLambdaPos;
|
||||
impl::applyPseudoImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaPos[i] );
|
||||
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
|
||||
penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);
|
||||
|
||||
pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross(a.pseudoAngularVelocity, rA);
|
||||
pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
|
||||
float pseudoVelAlongNormal = uf::vector::dot(pseudoVb - pseudoVa, contact.normal);
|
||||
|
||||
float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
|
||||
float jPseudo = (penetrationBias - pseudoVelAlongNormal) / invMassN;
|
||||
|
||||
float jPseudoOld = contact.accumulatedPseudoImpulse;
|
||||
float jPseudoNew = std::max(0.0f, jPseudoOld + jPseudo);
|
||||
contact.accumulatedPseudoImpulse = jPseudoNew;
|
||||
jPseudo = jPseudoNew - jPseudoOld;
|
||||
|
||||
impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal * jPseudo);
|
||||
}
|
||||
// tangent friction
|
||||
{
|
||||
|
||||
@ -8,7 +8,7 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
|
||||
|
||||
float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
|
||||
|
||||
// real impulse
|
||||
// normal impulse
|
||||
{
|
||||
pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
|
||||
pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
|
||||
@ -33,7 +33,7 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
|
||||
// pseudo impulse
|
||||
{
|
||||
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
|
||||
penetrationBias = std::min(penetrationBias, 2.0f / dt);
|
||||
penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);
|
||||
|
||||
pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross(a.pseudoAngularVelocity, rA);
|
||||
pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
|
||||
@ -48,7 +48,6 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
|
||||
|
||||
impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal * jPseudo);
|
||||
}
|
||||
|
||||
// tangent friction
|
||||
{
|
||||
pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user