Fixes 07 (#97)
#84 #82 Added B3_RESTITUTION_ITERATIONS to override restitution iterations. Fix regression in hull stacking stability.
This commit is contained in:
parent
dfa5e6add0
commit
23861418d8
27 changed files with 877 additions and 448 deletions
|
|
@ -84,6 +84,14 @@ option(BOX3D_DISABLE_SIMD "Disable SIMD math (slower)" OFF)
|
|||
option(BOX3D_COMPILE_WARNING_AS_ERROR "Compile warnings as errors" OFF)
|
||||
option(BOX3D_DOUBLE_PRECISION "Enable double precision for large worlds" OFF)
|
||||
|
||||
# These are read by the library build, so they must precede add_subdirectory(src),
|
||||
# otherwise the first configure and a re-configure produce different libraries.
|
||||
# Top level only, so they stay hidden from FetchContent consumers.
|
||||
if(PROJECT_IS_TOP_LEVEL)
|
||||
option(BOX3D_PROFILE "Enable profiling with Tracy" OFF)
|
||||
option(BOX3D_VALIDATE "Enable heavy validation" ON)
|
||||
endif()
|
||||
|
||||
# Emscripten maps the SSE2 path onto wasm SIMD128. simd.h is a header included by
|
||||
# non-library targets like the tests, so these flags must be directory-wide, not
|
||||
# PRIVATE to the box3d library.
|
||||
|
|
@ -127,8 +135,6 @@ if(PROJECT_IS_TOP_LEVEL)
|
|||
option(BOX3D_SAMPLES "Build the Box3D samples" ON)
|
||||
option(BOX3D_BENCHMARKS "Build the Box3D benchmarks" OFF)
|
||||
option(BOX3D_DOCS "Build the Box3D documentation" OFF)
|
||||
option(BOX3D_PROFILE "Enable profiling with Tracy" OFF)
|
||||
option(BOX3D_VALIDATE "Enable heavy validation" ON)
|
||||
option(BOX3D_UNIT_TESTS "Build the Box3D unit tests" ON)
|
||||
|
||||
# Enable this for shader development using sokol-shdc.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
# Collision
|
||||
|
||||
Box3D provides geometric types and functions. These include:
|
||||
|
||||
- primitives: spheres, capsules, and convex hulls
|
||||
- triangle meshes and height fields for static terrain
|
||||
- convex hull construction from point clouds
|
||||
|
|
@ -24,6 +25,7 @@ physics simulation. At a minimum, you should understand how to create primitives
|
|||
that can later be attached to rigid bodies.
|
||||
|
||||
Box3D shape primitives support several operations:
|
||||
|
||||
- Test a point or proxy for overlap with the primitive
|
||||
- Perform a ray cast against the primitive
|
||||
- Compute the primitive's bounding box
|
||||
|
|
@ -381,6 +383,7 @@ this for stable stacking.
|
|||
|
||||
Normally you do not compute manifolds directly — you use the contact data
|
||||
returned by the simulation. The `b3Manifold` struct contains:
|
||||
|
||||
- `normal`: unit normal from shape A toward shape B
|
||||
- `points[B3_MAX_MANIFOLD_POINTS]`: contact points with separation, impulses,
|
||||
and feature ids
|
||||
|
|
@ -401,15 +404,16 @@ b3CollideHulls(&manifold, B3_MAX_MANIFOLD_POINTS,
|
|||
```
|
||||
|
||||
Available collide functions:
|
||||
|
||||
- `b3CollideSpheres`
|
||||
- `b3CollideCapsuleAndSphere`
|
||||
- `b3CollideCapsules`
|
||||
- `b3CollideHullAndSphere`
|
||||
- `b3CollideHullAndCapsule`
|
||||
- `b3CollideHulls`
|
||||
- `b3CollideCapsuleAndTriangle`
|
||||
- `b3CollideHullAndTriangle`
|
||||
- `b3CollideSphereAndTriangle`
|
||||
- `b3CollideTriangleAndCapsule`
|
||||
- `b3CollideTriangleAndHull`
|
||||
- `b3CollideTriangleAndSphere`
|
||||
|
||||
The SAT cache in `b3CollideHulls` warm-starts the separating axis search between
|
||||
frames, the same idea as the simplex cache for GJK distance.
|
||||
|
|
|
|||
|
|
@ -44,7 +44,8 @@ B3_API uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId
|
|||
B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits,
|
||||
b3TreeQueryCallbackFcn* callback, void* context );
|
||||
|
||||
/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied point.
|
||||
/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied
|
||||
/// point.
|
||||
/// @param tree the dynamic tree to query
|
||||
/// @param point the query point
|
||||
/// @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero
|
||||
|
|
@ -634,17 +635,17 @@ B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, co
|
|||
B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3SATCache* cache );
|
||||
|
||||
/// Collide a capsule and a triangle.
|
||||
B3_API void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA,
|
||||
const b3Vec3* triangleB, b3SimplexCache* cache );
|
||||
/// Collide a triangle and capsule. Normal points from triangle to capsule.
|
||||
B3_API void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA,
|
||||
const b3Capsule* capsuleB, b3SimplexCache* cache );
|
||||
|
||||
/// Collide a hull and a triangle.
|
||||
B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2,
|
||||
b3Vec3 v3, int triangleFlags, b3SATCache* cache, bool enableSpeculative );
|
||||
/// Collide a triangle and hull. Normal points from triangle to hull.
|
||||
B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,
|
||||
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative );
|
||||
|
||||
/// Collide a sphere and a triangle.
|
||||
B3_API void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA,
|
||||
const b3Vec3* triangleB );
|
||||
/// Collide a triangle and sphere. Normal points from triangle to sphere.
|
||||
B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA,
|
||||
const b3Sphere* sphereB );
|
||||
|
||||
/**@}*/ // collision
|
||||
|
||||
|
|
|
|||
|
|
@ -31,3 +31,5 @@
|
|||
|
||||
// Enable assertions when NDEBUG is defined
|
||||
// #define B3_ENABLE_ASSERT
|
||||
|
||||
// #define B3_RESTITUTION_ITERATIONS
|
||||
|
|
|
|||
|
|
@ -131,3 +131,9 @@ B3_API float b3GetStallThreshold( void );
|
|||
#define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER )
|
||||
#define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER )
|
||||
#define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER )
|
||||
|
||||
/// Increase this if your application needs more accurate restitution. Doing so will
|
||||
/// slow down the simulation. Must be 1 or more.
|
||||
#ifndef B3_RESTITUTION_ITERATIONS
|
||||
#define B3_RESTITUTION_ITERATIONS 1
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -98,8 +98,8 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
|
|||
float drop = control * m_friction * timeStep;
|
||||
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
|
||||
float ratio = newSpeed / speed;
|
||||
m_velocity.x *= newSpeed / speed;
|
||||
m_velocity.z *= newSpeed / speed;
|
||||
m_velocity.x *= ratio;
|
||||
m_velocity.z *= ratio;
|
||||
}
|
||||
|
||||
float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed;
|
||||
|
|
|
|||
|
|
@ -1,13 +1,12 @@
|
|||
// SPDX-FileCopyrightText: 2025 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "gfx/debug_adapter.h"
|
||||
#include "gfx/draw.h"
|
||||
#include "imgui.h"
|
||||
#include "sample.h"
|
||||
#include "gfx/draw.h"
|
||||
#include "utils.h"
|
||||
|
||||
#include "gfx/debug_adapter.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
|
@ -442,7 +441,7 @@ public:
|
|||
m_runCount = 0;
|
||||
m_failure = false;
|
||||
m_autoGenerate = false;
|
||||
|
||||
m_collide = true;
|
||||
m_stepWhilePaused = true;
|
||||
|
||||
Generate();
|
||||
|
|
@ -549,13 +548,16 @@ public:
|
|||
|
||||
// Don't allow shapes to collide with each other. This
|
||||
// makes isolating failures easier.
|
||||
shapeDef.filter.categoryBits = 2;
|
||||
shapeDef.filter.maskBits = 1;
|
||||
if ( m_collide == false )
|
||||
{
|
||||
shapeDef.filter.categoryBits = 2;
|
||||
shapeDef.filter.maskBits = 1;
|
||||
}
|
||||
|
||||
g_randomSeed = (uint32_t)b3GetTicks();
|
||||
|
||||
bool simulateAll = true;
|
||||
//g_randomSeed = 1910133196;
|
||||
// g_randomSeed = 1910133196;
|
||||
|
||||
m_runCount += 1;
|
||||
m_stepCount = 0;
|
||||
|
|
@ -624,6 +626,11 @@ public:
|
|||
Generate();
|
||||
}
|
||||
|
||||
if ( ImGui::Checkbox( "Collide", &m_collide ) )
|
||||
{
|
||||
Generate();
|
||||
}
|
||||
|
||||
if ( ImGui::Button( "Generate" ) )
|
||||
{
|
||||
Generate();
|
||||
|
|
@ -645,8 +652,7 @@ public:
|
|||
{
|
||||
PickRay pickRay = m_camera->BuildPickRay( m_context->mouseX, m_context->mouseY );
|
||||
|
||||
b3RayResult result =
|
||||
b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
|
||||
b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
|
||||
|
||||
if ( result.hit )
|
||||
{
|
||||
|
|
@ -736,6 +742,7 @@ public:
|
|||
int m_runCount;
|
||||
bool m_failure;
|
||||
bool m_autoGenerate;
|
||||
bool m_collide;
|
||||
};
|
||||
|
||||
static int sampleMeshDrop = RegisterSample( "Continuous", "Mesh Drop", MeshDrop::Create );
|
||||
|
|
@ -862,13 +869,12 @@ public:
|
|||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 15.0f, 0.0f } );
|
||||
|
||||
}
|
||||
|
||||
AddGroundBox( 40.0f );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 10.0f, 0.5f );
|
||||
b3Capsule capsule = {{0.0f, -9.5f, 0.0f}, {0.0f, 9.5f, 0.0f}, 0.5f};
|
||||
b3Capsule capsule = { { 0.0f, -9.5f, 0.0f }, { 0.0f, 9.5f, 0.0f }, 0.5f };
|
||||
|
||||
bool useCapsule = false;
|
||||
|
||||
|
|
@ -881,7 +887,7 @@ public:
|
|||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
if (useCapsule)
|
||||
if ( useCapsule )
|
||||
{
|
||||
b3CreateCapsuleShape( bodyId, &shapeDef, &capsule );
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ public:
|
|||
{
|
||||
Sample::Step( );
|
||||
|
||||
if ( m_done == false )
|
||||
if ( m_done == false && (m_context->pause == false || m_context->singleStep > 0) )
|
||||
{
|
||||
m_done = UpdateFallingRagdolls( m_worldId, &m_data );
|
||||
if (m_done)
|
||||
|
|
|
|||
|
|
@ -1146,3 +1146,160 @@ public:
|
|||
};
|
||||
|
||||
static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create );
|
||||
|
||||
class RestitutionOvershoot : public Sample
|
||||
{
|
||||
public:
|
||||
static constexpr float m_boxHalf = 0.5f;
|
||||
static constexpr float m_floorHalfXZ = 0.375f;
|
||||
static constexpr float m_floorHalfY = 0.25f;
|
||||
static constexpr float m_dropHeight = 10.0f;
|
||||
static constexpr float m_tolerance = 0.05f;
|
||||
|
||||
explicit RestitutionOvershoot( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 20.0f, 0.0f, 28.0f, { 0.0f, m_dropHeight + m_boxHalf, 0.0f } );
|
||||
}
|
||||
|
||||
b3BoxHull floor = b3MakeBoxHull( m_floorHalfXZ, m_floorHalfY, m_floorHalfXZ );
|
||||
b3BodyDef floorDef = b3DefaultBodyDef();
|
||||
floorDef.type = b3_staticBody;
|
||||
floorDef.position = { 0.0f, -m_floorHalfY, 0.0f };
|
||||
b3BodyId floorBody = b3CreateBody( m_worldId, &floorDef );
|
||||
|
||||
b3ShapeDef floorShape = b3DefaultShapeDef();
|
||||
b3CreateHullShape( floorBody, &floorShape, &floor.base );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( m_boxHalf, m_boxHalf, m_boxHalf );
|
||||
b3BodyDef boxDef = b3DefaultBodyDef();
|
||||
boxDef.type = b3_dynamicBody;
|
||||
boxDef.position = { 0.0f, m_dropHeight, 0.0f };
|
||||
m_boxBody = b3CreateBody( m_worldId, &boxDef );
|
||||
|
||||
b3ShapeDef boxShape = b3DefaultShapeDef();
|
||||
boxShape.baseMaterial.restitution = 1.0f;
|
||||
b3CreateHullShape( m_boxBody, &boxShape, &box.base );
|
||||
|
||||
m_currentY = m_dropHeight;
|
||||
m_maxBounceY = 0.0f;
|
||||
m_bounced = false;
|
||||
m_failed = false;
|
||||
}
|
||||
|
||||
void Step() override
|
||||
{
|
||||
Sample::Step();
|
||||
|
||||
if ( B3_IS_NON_NULL( m_boxBody ) == false )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
b3Pos position = b3Body_GetPosition( m_boxBody );
|
||||
m_currentY = position.y;
|
||||
|
||||
b3Vec3 velocity = b3Body_GetLinearVelocity( m_boxBody );
|
||||
if ( m_bounced == false && velocity.y > 0.0f )
|
||||
{
|
||||
m_bounced = true;
|
||||
}
|
||||
|
||||
if ( m_bounced )
|
||||
{
|
||||
if ( position.y > m_maxBounceY )
|
||||
{
|
||||
m_maxBounceY = position.y;
|
||||
}
|
||||
if ( position.y > m_dropHeight + m_tolerance )
|
||||
{
|
||||
m_failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f };
|
||||
DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) );
|
||||
|
||||
DrawTextLine( "drop height = %.2f m", m_dropHeight );
|
||||
DrawTextLine( "current y = %.2f m", m_currentY );
|
||||
DrawTextLine( "max bounce = %.2f m", m_maxBounceY );
|
||||
|
||||
if ( m_bounced == false )
|
||||
{
|
||||
DrawTextLine( "waiting for first bounce..." );
|
||||
}
|
||||
else if ( m_failed )
|
||||
{
|
||||
DrawTextLine( "FAIL: box exceeded drop height" );
|
||||
}
|
||||
else
|
||||
{
|
||||
DrawTextLine( "PASS: bounce stays at or below drop height" );
|
||||
}
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new RestitutionOvershoot( context );
|
||||
}
|
||||
|
||||
b3BodyId m_boxBody = {};
|
||||
float m_currentY = 0.0f;
|
||||
float m_maxBounceY = 0.0f;
|
||||
bool m_bounced = false;
|
||||
bool m_failed = false;
|
||||
};
|
||||
|
||||
static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create );
|
||||
|
||||
class SlideTwistOffCenterShape : public Sample
|
||||
{
|
||||
public:
|
||||
explicit SlideTwistOffCenterShape( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( -30.0f, 17.0f, 30.0f, { 0.0f, 5.0f, 0.0f } );
|
||||
}
|
||||
|
||||
AddGroundBox( 50.0f );
|
||||
|
||||
b3Quat orientation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 20.0f * B3_DEG_TO_RAD );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
||||
bodyDef.position = { 0.0f, 4.0f, 0.0f };
|
||||
bodyDef.rotation = orientation;
|
||||
b3BodyId planeBody = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3BoxHull plane = b3MakeBoxHull( 10.0f, 0.5f, 10.0f );
|
||||
shapeDef.baseMaterial.friction = 0.6f;
|
||||
b3CreateHullShape( planeBody, &shapeDef, &plane.base );
|
||||
|
||||
b3Vec3 boxLocalCenter = { 1.0f, 0.5f, 1.0f };
|
||||
b3Vec3 boxOffset = b3RotateVector( orientation, boxLocalCenter );
|
||||
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { -boxOffset.x, 5.0f - boxOffset.y, -boxOffset.z };
|
||||
bodyDef.rotation = orientation;
|
||||
//bodyDef.angularVelocity = 25.0f * b3RotateVector( orientation, b3Vec3_axisY );
|
||||
b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3BoxHull mBox = b3MakeOffsetBoxHull( 1.0f, 0.5f, 1.0f, boxLocalCenter );
|
||||
shapeDef.baseMaterial.friction = 0.3f;
|
||||
b3CreateHullShape( boxBody, &shapeDef, &mBox.base );
|
||||
|
||||
b3Body_SetAngularVelocity( boxBody, 25.0f * b3RotateVector( orientation, b3Vec3_axisY ) );
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new SlideTwistOffCenterShape( context );
|
||||
}
|
||||
};
|
||||
|
||||
static int sampleSlideTwistOffCenterShape =
|
||||
RegisterSample( "Issues", "Slide Twist Off Center Shape", SlideTwistOffCenterShape::Create );
|
||||
|
|
|
|||
|
|
@ -517,7 +517,7 @@ public:
|
|||
b3TransformPoint( xf, m_triangle[2] ),
|
||||
};
|
||||
|
||||
b3CollideSphereAndTriangle( &m_manifold, m_pointCapacity, &m_sphere, localTriangle );
|
||||
b3CollideTriangleAndSphere( &m_manifold, m_pointCapacity, localTriangle, &m_sphere );
|
||||
}
|
||||
|
||||
b3Sphere m_sphere;
|
||||
|
|
@ -668,7 +668,7 @@ public:
|
|||
b3TransformPoint( xf, m_triangle[2] ),
|
||||
};
|
||||
|
||||
b3CollideCapsuleAndTriangle( &m_manifold, m_pointCapacity, &m_capsule, localTriangle, &m_simplexCache );
|
||||
b3CollideTriangleAndCapsule( &m_manifold, m_pointCapacity, localTriangle, &m_capsule, &m_simplexCache );
|
||||
}
|
||||
|
||||
b3Capsule m_capsule;
|
||||
|
|
@ -913,8 +913,8 @@ public:
|
|||
b3TransformPoint( xf, m_triangle[2] ),
|
||||
};
|
||||
|
||||
b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle[0], localTriangle[1], localTriangle[2],
|
||||
m_flags, &m_satCache, true );
|
||||
b3CollideTriangleAndHull( &m_manifold, m_pointCapacity, localTriangle[0], localTriangle[1], localTriangle[2],
|
||||
m_flags, m_hull, &m_satCache, true );
|
||||
}
|
||||
|
||||
int m_flags;
|
||||
|
|
|
|||
|
|
@ -1,10 +1,11 @@
|
|||
// SPDX-FileCopyrightText: 2025 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "gfx/draw.h"
|
||||
#include "human.h"
|
||||
#include "imgui.h"
|
||||
#include "sample.h"
|
||||
#include "gfx/draw.h"
|
||||
#include "utils.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
|
|
@ -232,9 +233,14 @@ public:
|
|||
m_groundMesh = b3CreateGridMesh( 20, 20, 1.0f, 1, true );
|
||||
b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, b3Vec3_one );
|
||||
|
||||
g_randomSeed = 42;
|
||||
float a = 0.1f * e_count;
|
||||
b3Vec3 lower = { -a, -a, -a };
|
||||
b3Vec3 upper = { a, a, a };
|
||||
for ( int i = 0; i < e_count; ++i )
|
||||
{
|
||||
b3Pos position = { 0.1f * i, 2.0f + 0.5f * i, -0.1f * i };
|
||||
b3Vec3 offset = RandomVec3( lower, upper );
|
||||
b3Pos position = { offset.x, 2.0f, offset.z };
|
||||
float torque = 10.0f;
|
||||
float hertz = 0.5f;
|
||||
float damping = 0.7f;
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ public:
|
|||
m_camera->SetView( -140.0f, 17.0f, 60.0f, { 0.0f, 7.5f, 0.0f } );
|
||||
}
|
||||
|
||||
AddGroundBox( 50.0f );
|
||||
AddGroundBox( 80.0f );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
|
|
|||
|
|
@ -210,7 +210,7 @@ public:
|
|||
bodyDef.name = "cube";
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { 0.0f, 0.5f, 0.0f };
|
||||
//bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f };
|
||||
// bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f };
|
||||
m_bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
|
@ -409,7 +409,7 @@ public:
|
|||
{
|
||||
if ( m_context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 35.0f, 15.0f, 30.0f, { 0.0f, 10.0f, 0.0f } );
|
||||
m_camera->SetView( 35.0f, 15.0f, 12.0f, { 0.0f, 2.0f, 0.0f } );
|
||||
}
|
||||
|
||||
m_shapeType = b3_hullShape;
|
||||
|
|
@ -418,42 +418,43 @@ public:
|
|||
|
||||
void CreateStack()
|
||||
{
|
||||
AddGroundBox( 60.0f );
|
||||
AddGroundBox( 20.0f );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.05f;
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
|
||||
float h = 1.0f;
|
||||
float r = 0.1f;
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( h, r, r );
|
||||
b3Capsule capsule = { { -h, 0.0f, 0.0f }, { h, 0.0f, 0.0f }, r };
|
||||
int count = 30;
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.01f;
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI;
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 2.5f, 0.25f, 0.25f );
|
||||
b3Capsule capsule = { { -2.5f, 0.0f, 0.0f }, { 2.5f, 0.0f, 0.0f }, 0.25f };
|
||||
float x = ( i & 1 ) == 0 ? h - 2.0f * r : 0.0f;
|
||||
float z = ( i & 1 ) == 0 ? 0.0f : h - 2.0f * r;
|
||||
|
||||
for ( int i = 0; i < m_size; ++i )
|
||||
bodyDef.position = { x, ( 2.1f * i + 0.5f ) * r, z };
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
|
||||
b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
bodyDef.position = { -x, ( 2.1f * i + 0.5f ) * r, -z };
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
|
||||
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
if ( m_shapeType == b3_capsuleShape )
|
||||
{
|
||||
float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI;
|
||||
|
||||
float x = ( i & 1 ) == 0 ? 1.75f : 0.0f;
|
||||
float z = ( i & 1 ) == 0 ? 0.0f : 1.75f;
|
||||
|
||||
bodyDef.position = { x, 0.5f * i + 0.25f, z };
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
|
||||
b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
bodyDef.position = { -x, 0.5f * i + 0.25f, -z };
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
|
||||
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
if ( m_shapeType == b3_capsuleShape )
|
||||
{
|
||||
b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule );
|
||||
b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule );
|
||||
}
|
||||
else
|
||||
{
|
||||
b3CreateHullShape( boxBody1, &shapeDef, &box.base );
|
||||
b3CreateHullShape( boxBody2, &shapeDef, &box.base );
|
||||
}
|
||||
b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule );
|
||||
b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule );
|
||||
}
|
||||
else
|
||||
{
|
||||
b3CreateHullShape( boxBody1, &shapeDef, &box.base );
|
||||
b3CreateHullShape( boxBody2, &shapeDef, &box.base );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -484,7 +485,6 @@ public:
|
|||
return new JengaStack( context );
|
||||
}
|
||||
|
||||
static constexpr int m_size = 40;
|
||||
b3ShapeType m_shapeType;
|
||||
};
|
||||
|
||||
|
|
@ -811,7 +811,7 @@ public:
|
|||
{
|
||||
if ( m_context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 0.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } );
|
||||
m_camera->SetView( 40.0f, 15.0f, 50.0f, { 0.0f, 12.0f, 0.0f } );
|
||||
}
|
||||
|
||||
AddGroundBox( 40.0f );
|
||||
|
|
@ -847,3 +847,93 @@ public:
|
|||
};
|
||||
|
||||
static int samplePyramid2D = RegisterSample( "Stacking", "Pyramid2D", Pyramid2D::Create );
|
||||
|
||||
// This stresses the edge-edge handling.
|
||||
class EdgeCrossing : public Sample
|
||||
{
|
||||
public:
|
||||
explicit EdgeCrossing( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Pos_zero );
|
||||
}
|
||||
|
||||
AddGroundBox( 40.0f );
|
||||
|
||||
b3Vec3 h = { 0.2f, 0.02f, 0.04f };
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
||||
bodyDef.position.x = -10.0f;
|
||||
bodyDef.position.z = -2.0f;
|
||||
|
||||
b3BoxHull box1 = b3MakeBoxHull( h.x, h.y, h.z );
|
||||
b3BoxHull box2 = b3MakeBoxHull( h.x, h.z, h.y );
|
||||
|
||||
b3Vec3 axis = b3Normalize( { 0.1f, 0.9f, 0.0f } );
|
||||
for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI )
|
||||
{
|
||||
bodyDef.position.y = h.y;
|
||||
bodyDef.rotation = b3Quat_identity;
|
||||
b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( bodyId1, &shapeDef, &box1.base );
|
||||
|
||||
bodyDef.position.y = 20.0f * h.y;
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
|
||||
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3CreateHullShape( boxBody2, &shapeDef, &box1.base );
|
||||
|
||||
bodyDef.position.x += 1.0f;
|
||||
}
|
||||
|
||||
bodyDef.position.x = -10.0f;
|
||||
bodyDef.position.z = 0.0f;
|
||||
|
||||
for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI )
|
||||
{
|
||||
bodyDef.position.y = h.z;
|
||||
bodyDef.rotation = b3Quat_identity;
|
||||
b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( bodyId1, &shapeDef, &box2.base );
|
||||
|
||||
bodyDef.position.y = 20.0f * h.z;
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
|
||||
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3CreateHullShape( boxBody2, &shapeDef, &box2.base );
|
||||
|
||||
bodyDef.position.x += 1.0f;
|
||||
}
|
||||
|
||||
bodyDef.position.x = -10.0f;
|
||||
bodyDef.position.z = 2.0f;
|
||||
|
||||
for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI )
|
||||
{
|
||||
bodyDef.position.y = h.y;
|
||||
bodyDef.rotation = b3Quat_identity;
|
||||
b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( bodyId1, &shapeDef, &box1.base );
|
||||
|
||||
bodyDef.position.y = 20.0f * h.y;
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
|
||||
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3CreateHullShape( boxBody2, &shapeDef, &box2.base );
|
||||
|
||||
bodyDef.position.x += 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new EdgeCrossing( context );
|
||||
}
|
||||
};
|
||||
|
||||
static int sampleEdgeCrossing = RegisterSample( "Stacking", "Edge Crossing", EdgeCrossing::Create );
|
||||
|
|
|
|||
|
|
@ -4,7 +4,8 @@
|
|||
#include "utils.h"
|
||||
|
||||
#if defined( _WIN64 )
|
||||
#include <Windows.h>
|
||||
// Lower-case windows.h intentionally for cross compiling on mingw.
|
||||
#include <windows.h>
|
||||
#elif defined( __APPLE__ )
|
||||
#include <unistd.h>
|
||||
#elif defined( __linux__ )
|
||||
|
|
|
|||
|
|
@ -154,7 +154,8 @@ if (BOX3D_PROFILE)
|
|||
endif()
|
||||
|
||||
if(BOX3D_VALIDATE)
|
||||
message(STATUS "Box3D validation ON")
|
||||
# Only effective when NDEBUG is undefined, see base.h
|
||||
message(STATUS "Box3D validation ON (debug builds)")
|
||||
target_compile_definitions(box3d PRIVATE BOX3D_VALIDATE)
|
||||
endif()
|
||||
|
||||
|
|
|
|||
|
|
@ -1558,7 +1558,7 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context )
|
|||
int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );
|
||||
int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );
|
||||
int pointCount = b3MaxInt( pointCount1, pointCount2 );
|
||||
B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH );
|
||||
B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
|
||||
|
||||
// Normal impulses
|
||||
for ( int j = 0; j < pointCount; ++j )
|
||||
|
|
@ -1632,7 +1632,7 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u
|
|||
int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );
|
||||
int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );
|
||||
int pointCount = b3MaxInt( pointCount1, pointCount2 );
|
||||
B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH );
|
||||
B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
|
||||
|
||||
b3BodyStateW bA = b3GatherBodies( states, c->indexA );
|
||||
b3BodyStateW bB = b3GatherBodies( states, c->indexB );
|
||||
|
|
@ -1847,7 +1847,7 @@ void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context )
|
|||
int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );
|
||||
int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );
|
||||
int pointCount = b3MaxInt( pointCount1, pointCount2 );
|
||||
B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH );
|
||||
B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
|
||||
|
||||
// Single gather for all manifolds
|
||||
b3BodyStateW bA = b3GatherBodies( states, c->indexA );
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
// SPDX-FileCopyrightText: 2025 Erin Catto
|
||||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "algorithm.h"
|
||||
|
|
@ -82,8 +82,8 @@ static int b3ClipSegmentToHullFace( b3ClipVertex segment[2], const b3HullData* h
|
|||
return 2;
|
||||
}
|
||||
|
||||
static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
|
||||
b3Transform capsuleTransform )
|
||||
static b3SeparatingAxis b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
|
||||
b3Transform capsuleTransform )
|
||||
{
|
||||
int maxFaceIndex = -1;
|
||||
int maxVertexIndex = -1;
|
||||
|
|
@ -110,15 +110,16 @@ static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, c
|
|||
}
|
||||
}
|
||||
|
||||
return (b3FaceQuery){
|
||||
return (b3SeparatingAxis){
|
||||
.normal = planes[maxFaceIndex].normal,
|
||||
.separation = maxFaceSeparation,
|
||||
.faceIndex = (uint8_t)maxFaceIndex,
|
||||
.vertexIndex = (uint8_t)maxVertexIndex,
|
||||
.indexA = (uint8_t)maxFaceIndex,
|
||||
.indexB = (uint8_t)maxVertexIndex,
|
||||
};
|
||||
}
|
||||
|
||||
static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
|
||||
b3Transform capsuleTransform )
|
||||
static b3SeparatingAxis b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
|
||||
b3Transform capsuleTransform )
|
||||
{
|
||||
// Find axis of minimum penetration
|
||||
b3Vec3 maxNormal = b3Vec3_zero;
|
||||
|
|
@ -194,7 +195,7 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c
|
|||
}
|
||||
|
||||
// Save result
|
||||
return (b3EdgeQuery){
|
||||
return (b3SeparatingAxis){
|
||||
.normal = maxNormal,
|
||||
.separation = maxSeparation,
|
||||
.indexA = maxIndexA,
|
||||
|
|
@ -704,13 +705,13 @@ void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule
|
|||
}
|
||||
|
||||
static bool b3BuildHullFaceAndCapsuleContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3Capsule* capsuleB,
|
||||
b3Transform transformBtoA, b3FaceQuery query )
|
||||
b3Transform transformBtoA, b3SeparatingAxis query )
|
||||
{
|
||||
// Work in shapeA coordinates
|
||||
const b3Plane* planes = b3GetHullPlanes( hullA );
|
||||
|
||||
// Clip the capsule edge against the side planes of the reference face
|
||||
int refFace = query.faceIndex;
|
||||
int refFace = query.indexA;
|
||||
b3Plane refPlane = planes[refFace];
|
||||
|
||||
b3ClipVertex segmentB[2];
|
||||
|
|
@ -771,7 +772,7 @@ static inline float b3DeepestPointSeparation( const b3LocalManifold* manifold )
|
|||
}
|
||||
|
||||
static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA,
|
||||
const b3Capsule* capsuleB, b3Transform transformBtoA, b3EdgeQuery query )
|
||||
const b3Capsule* capsuleB, b3Transform transformBtoA, b3SeparatingAxis query )
|
||||
{
|
||||
if ( capacity < 1 )
|
||||
{
|
||||
|
|
@ -916,14 +917,14 @@ void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3H
|
|||
|
||||
// Deep penetration
|
||||
|
||||
b3FaceQuery faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
|
||||
b3SeparatingAxis faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
|
||||
if ( faceQuery.separation > capsuleB->radius )
|
||||
{
|
||||
// We found a separating axis
|
||||
return;
|
||||
}
|
||||
|
||||
b3EdgeQuery edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
|
||||
b3SeparatingAxis edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
|
||||
if ( edgeQuery.separation > capsuleB->radius )
|
||||
{
|
||||
// We found a separating axis
|
||||
|
|
@ -999,20 +1000,24 @@ static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3Hul
|
|||
}
|
||||
|
||||
static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache )
|
||||
b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )
|
||||
{
|
||||
B3_VALIDATE( query.type == b3_faceAxisA );
|
||||
B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->faceCount );
|
||||
B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->vertexCount );
|
||||
|
||||
const b3HullFace* facesA = b3GetHullFaces( hullA );
|
||||
const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA );
|
||||
const b3Plane* planesA = b3GetHullPlanes( hullA );
|
||||
const b3Vec3* pointsA = b3GetHullPoints( hullA );
|
||||
|
||||
// Reference face
|
||||
int refFace = query.faceIndex;
|
||||
int refFace = query.indexA;
|
||||
b3Plane refPlane = planesA[refFace];
|
||||
|
||||
// Find incident face
|
||||
b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal );
|
||||
int incFace = b3FindIncidentFace( hullB, refNormalInB, query.vertexIndex );
|
||||
int incFace = b3FindIncidentFace( hullB, refNormalInB, query.indexB );
|
||||
|
||||
// Build clip polygon from incident face in frame A
|
||||
b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS];
|
||||
|
|
@ -1089,19 +1094,30 @@ static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const
|
|||
// Save cache
|
||||
cache->separation = minSeparation;
|
||||
cache->type = (uint8_t)b3_faceAxisA;
|
||||
cache->indexA = (uint8_t)query.faceIndex;
|
||||
cache->indexB = (uint8_t)query.vertexIndex;
|
||||
cache->indexA = (uint8_t)query.indexA;
|
||||
cache->indexB = (uint8_t)query.indexB;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache )
|
||||
b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )
|
||||
{
|
||||
B3_VALIDATE( query.type == b3_faceAxisB );
|
||||
|
||||
b3Transform transformAtoB = b3InvertTransform( transformBtoA );
|
||||
bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, query, cache );
|
||||
b3SeparatingAxis flippedQuery = {
|
||||
.normal = b3Neg( query.normal ),
|
||||
.separation = query.separation,
|
||||
.indexA = query.indexB,
|
||||
.indexB = query.indexA,
|
||||
.type = b3_faceAxisA,
|
||||
};
|
||||
|
||||
bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, flippedQuery, cache );
|
||||
if ( touching == false )
|
||||
{
|
||||
*cache = (b3SATCache){ 0 };
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1110,9 +1126,6 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const
|
|||
|
||||
// Transform and flip normal so it points from A to B, even though the B has the reference face.
|
||||
manifold->normal = b3Neg( b3MulMV( matrix, manifold->normal ) );
|
||||
cache->type = (uint8_t)b3_faceAxisB;
|
||||
cache->indexA = (uint8_t)query.vertexIndex;
|
||||
cache->indexB = (uint8_t)query.faceIndex;
|
||||
|
||||
// Transform points from frame B to frame A.
|
||||
// Also flip the pairs to ensure correct matches.
|
||||
|
|
@ -1123,12 +1136,20 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const
|
|||
pt->pair = b3FlipPair( pt->pair );
|
||||
}
|
||||
|
||||
cache->type = (uint8_t)b3_faceAxisB;
|
||||
cache->indexA = (uint8_t)query.indexA;
|
||||
cache->indexB = (uint8_t)query.indexB;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3EdgeQuery query, b3SATCache* cache )
|
||||
b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )
|
||||
{
|
||||
B3_VALIDATE( query.type == b3_edgePairAxis );
|
||||
B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->edgeCount );
|
||||
B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->edgeCount );
|
||||
|
||||
// Work in shapeA coordinates
|
||||
const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA );
|
||||
const b3Vec3* pointsA = b3GetHullPoints( hullA );
|
||||
|
|
@ -1295,22 +1316,39 @@ static inline void b3GetSupportWide( b3Vec3 normal, const float* vx, const float
|
|||
|
||||
// SIMD separating axis test based on an implementation developed by Cairn Overturf.
|
||||
// See his article: https://cairno.substack.com/p/improvements-to-the-separating-axis
|
||||
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride )
|
||||
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn )
|
||||
{
|
||||
B3_VALIDATE( axisOverride == b3_invalidAxis || axisOverride == b3_manualFaceAxisA || axisOverride == b3_manualFaceAxisB ||
|
||||
axisOverride == b3_manualEdgePairAxis );
|
||||
|
||||
b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q );
|
||||
b3Matrix3 invR = b3Transpose( R );
|
||||
|
||||
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
|
||||
|
||||
b3AxisQuery res = {
|
||||
.normal = b3Vec3_zero,
|
||||
.separation = -INFINITY,
|
||||
.indexA = B3_NULL_INDEX,
|
||||
.indexB = B3_NULL_INDEX,
|
||||
.type = b3_invalidAxis,
|
||||
.faceA =
|
||||
{
|
||||
.normal = b3Vec3_zero,
|
||||
.separation = -INFINITY,
|
||||
.indexA = B3_NULL_INDEX,
|
||||
.indexB = B3_NULL_INDEX,
|
||||
.type = b3_faceAxisA,
|
||||
},
|
||||
.faceB =
|
||||
{
|
||||
.normal = b3Vec3_zero,
|
||||
.separation = -INFINITY,
|
||||
.indexA = B3_NULL_INDEX,
|
||||
.indexB = B3_NULL_INDEX,
|
||||
.type = b3_faceAxisB,
|
||||
},
|
||||
.edge =
|
||||
{
|
||||
.normal = b3Vec3_zero,
|
||||
.separation = -INFINITY,
|
||||
.indexA = B3_NULL_INDEX,
|
||||
.indexB = B3_NULL_INDEX,
|
||||
.type = b3_edgePairAxis,
|
||||
},
|
||||
.separatedFeature = b3_invalidAxis,
|
||||
};
|
||||
|
||||
int faceCountA = hullA->faceCount;
|
||||
|
|
@ -1325,38 +1363,30 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
b3Vec3 hB = b3AABB_Extents( hullB->aabb );
|
||||
|
||||
// Test A's face planes against B's vertices.
|
||||
if ( axisOverride != b3_manualFaceAxisB && axisOverride != b3_manualEdgePairAxis )
|
||||
for ( int i = 0; i < faceCountA; ++i )
|
||||
{
|
||||
for ( int i = 0; i < faceCountA; ++i )
|
||||
b3Plane plane = planesA[i];
|
||||
b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) );
|
||||
float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset;
|
||||
float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB );
|
||||
float support;
|
||||
int vertexIndex;
|
||||
b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex );
|
||||
float separation = planeSeparation - support;
|
||||
if ( separation > res.faceA.separation )
|
||||
{
|
||||
b3Plane plane = planesA[i];
|
||||
b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) );
|
||||
float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset;
|
||||
float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB );
|
||||
float support;
|
||||
int vertexIndex;
|
||||
b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex );
|
||||
float separation = planeSeparation - support;
|
||||
if ( separation > res.separation )
|
||||
res.faceA.normal = plane.normal;
|
||||
res.faceA.separation = separation;
|
||||
res.faceA.indexA = i;
|
||||
res.faceA.indexB = vertexIndex;
|
||||
if ( separation > speculativeDistance && earlyReturn )
|
||||
{
|
||||
res.type = b3_faceAxisA;
|
||||
res.separation = separation;
|
||||
res.indexA = i;
|
||||
res.indexB = vertexIndex;
|
||||
res.normal = plane.normal;
|
||||
if ( separation > speculativeDistance )
|
||||
{
|
||||
return res;
|
||||
}
|
||||
res.separatedFeature = b3_faceAxisA;
|
||||
return res;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( axisOverride == b3_manualFaceAxisA )
|
||||
{
|
||||
return res;
|
||||
}
|
||||
|
||||
int faceCountB = hullB->faceCount;
|
||||
const b3Plane* planesB = b3GetHullPlanes( hullB );
|
||||
|
||||
|
|
@ -1369,39 +1399,31 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
b3Vec3 hA = b3AABB_Extents( hullA->aabb );
|
||||
|
||||
// Test B's face planes against A's vertices.
|
||||
if ( axisOverride != b3_manualEdgePairAxis )
|
||||
for ( int i = 0; i < faceCountB; ++i )
|
||||
{
|
||||
for ( int i = 0; i < faceCountB; ++i )
|
||||
b3Plane plane = planesB[i];
|
||||
b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) );
|
||||
float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset;
|
||||
float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA );
|
||||
float support;
|
||||
int vertexIndex;
|
||||
b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex );
|
||||
float separation = planeSeparation - support;
|
||||
if ( separation > res.faceB.separation )
|
||||
{
|
||||
b3Plane plane = planesB[i];
|
||||
b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) );
|
||||
float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset;
|
||||
float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA );
|
||||
float support;
|
||||
int vertexIndex;
|
||||
b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex );
|
||||
float separation = planeSeparation - support;
|
||||
if ( separation > res.separation )
|
||||
res.faceB.normal = direction;
|
||||
res.faceB.separation = separation;
|
||||
res.faceB.indexA = vertexIndex;
|
||||
res.faceB.indexB = i;
|
||||
// This points from A to B and is in frame A
|
||||
if ( separation > speculativeDistance && earlyReturn )
|
||||
{
|
||||
res.type = b3_faceAxisB;
|
||||
res.separation = separation;
|
||||
res.indexA = vertexIndex;
|
||||
res.indexB = i;
|
||||
// This points from A to B and is in frame A
|
||||
res.normal = direction;
|
||||
if ( separation > speculativeDistance )
|
||||
{
|
||||
return res;
|
||||
}
|
||||
res.separatedFeature = b3_faceAxisB;
|
||||
return res;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( axisOverride == b3_manualFaceAxisB )
|
||||
{
|
||||
return res;
|
||||
}
|
||||
|
||||
// Transform B into A's space once, into SoA arrays. Extra space so
|
||||
// tail can be set to zero in all cases.
|
||||
_Static_assert( ( B3_MAX_HULL_EDGES & 3 ) == 0, "must be multiple of 4" );
|
||||
|
|
@ -1536,9 +1558,6 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
b3StoreW( aV0z + na, zero );
|
||||
b3StoreW( aTol + na, zero );
|
||||
|
||||
// Prefer face contact for more contact points.
|
||||
float absFaceBias = 0.1f * B3_LINEAR_SLOP;
|
||||
|
||||
int edgeCountB = halfEdgeCountB / 2;
|
||||
|
||||
#if defined( B3_SIMD_NONE )
|
||||
|
|
@ -1608,20 +1627,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
float sz = aV0z[i] + bv0z;
|
||||
|
||||
float separation = -( sx * nx + ( sy * ny + sz * nz ) );
|
||||
if ( separation > res.separation + absFaceBias )
|
||||
if ( separation > res.edge.separation )
|
||||
{
|
||||
res.normal = (b3Vec3){ nx, ny, nz };
|
||||
res.separation = separation;
|
||||
res.type = b3_edgePairAxis;
|
||||
res.edge.normal = (b3Vec3){ nx, ny, nz };
|
||||
res.edge.separation = separation;
|
||||
|
||||
// Half edge index
|
||||
res.indexA = 2 * i;
|
||||
res.indexB = 2 * j;
|
||||
|
||||
// Edge beats face, remove bias
|
||||
absFaceBias = 0.0f;
|
||||
if ( separation > speculativeDistance )
|
||||
res.edge.indexA = 2 * i;
|
||||
res.edge.indexB = 2 * j;
|
||||
|
||||
if ( separation > speculativeDistance && earlyReturn )
|
||||
{
|
||||
res.separatedFeature = b3_edgePairAxis;
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
|
@ -1629,7 +1646,7 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
}
|
||||
|
||||
#else
|
||||
|
||||
|
||||
// Edge phase, one B edge against four A edges at a time, no transforms in the loop.
|
||||
const b3FloatW EPS = b3SplatW( -0.0001f );
|
||||
const b3FloatW INF = b3SplatW( INFINITY );
|
||||
|
|
@ -1718,7 +1735,7 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
// Test all 4 supports against the running best at once. If none beats it, skip the
|
||||
// store and scalar reduction. res->support only turns negative just before returning,
|
||||
// so this never skips a lane that would trigger the early out.
|
||||
b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.separation ) );
|
||||
b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.edge.separation ) );
|
||||
if ( b3AnyTrueW( improves ) == false )
|
||||
{
|
||||
continue;
|
||||
|
|
@ -1740,20 +1757,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
|
|||
{
|
||||
int ei = i + lane;
|
||||
float s = sA[lane];
|
||||
if ( s > res.separation + absFaceBias )
|
||||
if ( s > res.edge.separation )
|
||||
{
|
||||
res.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] };
|
||||
res.separation = s;
|
||||
res.type = b3_edgePairAxis;
|
||||
res.edge.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] };
|
||||
res.edge.separation = s;
|
||||
|
||||
// Half edge index
|
||||
res.indexA = 2 * ei;
|
||||
res.indexB = 2 * j;
|
||||
res.edge.indexA = 2 * ei;
|
||||
res.edge.indexB = 2 * j;
|
||||
|
||||
// Edge beats face, remove bias
|
||||
absFaceBias = 0.0f;
|
||||
|
||||
if ( s > speculativeDistance )
|
||||
if ( s > speculativeDistance && earlyReturn )
|
||||
{
|
||||
res.separatedFeature = b3_edgePairAxis;
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
|
@ -1824,10 +1839,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
}
|
||||
|
||||
// Attempt face contact using cached feature
|
||||
b3FaceQuery faceQuery;
|
||||
b3SeparatingAxis faceQuery;
|
||||
faceQuery.normal = plane.normal;
|
||||
faceQuery.separation = 0.0f;
|
||||
faceQuery.faceIndex = cache->indexA;
|
||||
faceQuery.vertexIndex = vertexIndex;
|
||||
faceQuery.indexA = cache->indexA;
|
||||
faceQuery.indexB = vertexIndex;
|
||||
faceQuery.type = b3_faceAxisA;
|
||||
|
||||
b3SATCache localCache = { 0 };
|
||||
bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache );
|
||||
|
|
@ -1861,10 +1878,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
}
|
||||
|
||||
// Attempt face contact using cached feature
|
||||
b3FaceQuery faceQuery;
|
||||
b3SeparatingAxis faceQuery;
|
||||
faceQuery.normal = b3Neg( plane.normal );
|
||||
faceQuery.separation = 0.0f;
|
||||
faceQuery.faceIndex = cache->indexB;
|
||||
faceQuery.vertexIndex = vertexIndex;
|
||||
faceQuery.indexA = vertexIndex;
|
||||
faceQuery.indexB = cache->indexB;
|
||||
faceQuery.type = b3_faceAxisB;
|
||||
|
||||
b3SATCache localCache = { 0 };
|
||||
bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache );
|
||||
|
|
@ -1933,11 +1952,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
}
|
||||
|
||||
// Try to rebuild contact from last features
|
||||
b3EdgeQuery edgeQuery = { 0 };
|
||||
b3SeparatingAxis edgeQuery = { 0 };
|
||||
edgeQuery.normal = b3Neg( axis );
|
||||
edgeQuery.separation = 0.0f;
|
||||
edgeQuery.indexA = cache->indexA;
|
||||
edgeQuery.indexB = cache->indexB;
|
||||
edgeQuery.type = b3_edgePairAxis;
|
||||
|
||||
b3SATCache localCache = { 0 };
|
||||
bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache );
|
||||
|
|
@ -1957,13 +1977,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
// This case is for testing
|
||||
case b3_manualFaceAxisA:
|
||||
{
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisA );
|
||||
b3FaceQuery faceQuery = {
|
||||
.separation = axisQuery.separation,
|
||||
.faceIndex = axisQuery.indexA,
|
||||
.vertexIndex = axisQuery.indexB,
|
||||
};
|
||||
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );
|
||||
b3SeparatingAxis faceQuery = axisQuery.faceA;
|
||||
b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
|
||||
return;
|
||||
}
|
||||
|
|
@ -1971,12 +1986,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
// This case is for testing
|
||||
case b3_manualFaceAxisB:
|
||||
{
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisB );
|
||||
b3FaceQuery faceQuery = {
|
||||
.separation = axisQuery.separation,
|
||||
.faceIndex = axisQuery.indexB,
|
||||
.vertexIndex = axisQuery.indexA,
|
||||
};
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );
|
||||
b3SeparatingAxis faceQuery = axisQuery.faceB;
|
||||
b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
|
||||
return;
|
||||
}
|
||||
|
|
@ -1984,14 +1995,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
// This case is for testing
|
||||
case b3_manualEdgePairAxis:
|
||||
{
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualEdgePairAxis );
|
||||
b3EdgeQuery edgeQuery = {
|
||||
.normal = axisQuery.normal,
|
||||
.indexA = axisQuery.indexA,
|
||||
.indexB = axisQuery.indexB,
|
||||
.separation = axisQuery.separation,
|
||||
};
|
||||
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );
|
||||
b3SeparatingAxis edgeQuery = axisQuery.edge;
|
||||
if ( edgeQuery.indexA != B3_NULL_INDEX )
|
||||
{
|
||||
b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache );
|
||||
|
|
@ -2007,74 +2012,94 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
manifold->pointCount = 0;
|
||||
*cache = (b3SATCache){ 0 };
|
||||
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_invalidAxis );
|
||||
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, true );
|
||||
|
||||
B3_VALIDATE( 0 <= axisQuery.indexA && axisQuery.indexA <= UINT8_MAX );
|
||||
B3_VALIDATE( 0 <= axisQuery.indexB && axisQuery.indexB <= UINT8_MAX );
|
||||
B3_ASSERT( axisQuery.type != b3_invalidAxis );
|
||||
|
||||
cache->separation = axisQuery.separation;
|
||||
cache->type = (uint8_t)axisQuery.type;
|
||||
cache->indexA = (uint8_t)axisQuery.indexA;
|
||||
cache->indexB = (uint8_t)axisQuery.indexB;
|
||||
|
||||
if ( axisQuery.separation > speculativeDistance )
|
||||
if ( axisQuery.separatedFeature != b3_invalidAxis )
|
||||
{
|
||||
// We found a separating axis
|
||||
cache->type = axisQuery.separatedFeature;
|
||||
|
||||
if ( axisQuery.separatedFeature == b3_faceAxisA )
|
||||
{
|
||||
B3_VALIDATE( axisQuery.faceA.separation > speculativeDistance );
|
||||
cache->separation = axisQuery.faceA.separation;
|
||||
cache->indexA = (uint8_t)axisQuery.faceA.indexA;
|
||||
cache->indexB = (uint8_t)axisQuery.faceA.indexB;
|
||||
}
|
||||
else if ( axisQuery.separatedFeature == b3_faceAxisB )
|
||||
{
|
||||
B3_VALIDATE( axisQuery.faceB.separation > speculativeDistance );
|
||||
cache->separation = axisQuery.faceB.separation;
|
||||
cache->indexA = (uint8_t)axisQuery.faceB.indexA;
|
||||
cache->indexB = (uint8_t)axisQuery.faceB.indexB;
|
||||
}
|
||||
else
|
||||
{
|
||||
B3_ASSERT( axisQuery.separatedFeature == b3_edgePairAxis );
|
||||
B3_VALIDATE( axisQuery.edge.separation > speculativeDistance );
|
||||
cache->separation = axisQuery.edge.separation;
|
||||
cache->indexA = (uint8_t)axisQuery.edge.indexA;
|
||||
cache->indexB = (uint8_t)axisQuery.edge.indexB;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if ( axisQuery.type == b3_faceAxisA )
|
||||
{
|
||||
B3_ASSERT( axisQuery.indexA < hullA->faceCount );
|
||||
B3_ASSERT( axisQuery.indexB < hullB->vertexCount );
|
||||
B3_VALIDATE( axisQuery.faceA.separation <= speculativeDistance || axisQuery.faceB.separation <= speculativeDistance ||
|
||||
axisQuery.edge.separation <= speculativeDistance );
|
||||
|
||||
b3FaceQuery faceQuery = {
|
||||
.separation = axisQuery.separation,
|
||||
.faceIndex = axisQuery.indexA,
|
||||
.vertexIndex = axisQuery.indexB,
|
||||
};
|
||||
if ( axisQuery.faceA.separation > axisQuery.faceB.separation )
|
||||
{
|
||||
b3SeparatingAxis faceQuery = axisQuery.faceA;
|
||||
B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->faceCount );
|
||||
B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->vertexCount );
|
||||
|
||||
// Face contact A
|
||||
b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
|
||||
|
||||
return;
|
||||
B3_VALIDATE( cache->indexA < hullA->faceCount );
|
||||
B3_VALIDATE( cache->indexB < hullB->vertexCount );
|
||||
}
|
||||
|
||||
if ( axisQuery.type == b3_faceAxisB )
|
||||
else
|
||||
{
|
||||
B3_ASSERT( axisQuery.indexA < hullA->vertexCount );
|
||||
B3_ASSERT( axisQuery.indexB < hullB->faceCount );
|
||||
|
||||
b3FaceQuery faceQuery = {
|
||||
.separation = axisQuery.separation,
|
||||
.faceIndex = axisQuery.indexB,
|
||||
.vertexIndex = axisQuery.indexA,
|
||||
};
|
||||
b3SeparatingAxis faceQuery = axisQuery.faceB;
|
||||
B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->vertexCount );
|
||||
B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->faceCount );
|
||||
|
||||
// Face contact B
|
||||
b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
|
||||
|
||||
B3_VALIDATE( cache->indexA < hullA->vertexCount );
|
||||
B3_VALIDATE( cache->indexB < hullB->faceCount );
|
||||
}
|
||||
|
||||
b3SeparatingAxis edgeQuery = axisQuery.edge;
|
||||
|
||||
if ( edgeQuery.indexA == B3_NULL_INDEX )
|
||||
{
|
||||
// There are no valid edge pairs (all edges parallel)
|
||||
return;
|
||||
}
|
||||
|
||||
B3_ASSERT( axisQuery.type == b3_edgePairAxis );
|
||||
float clipSeparation = cache->separation;
|
||||
float edgeTol = linearSlop;
|
||||
|
||||
// Face contact can be empty if it does not realize the axis of minimum penetration.
|
||||
// Create edge contact if face contact fails or edge contact is significantly better!
|
||||
if ( manifold->pointCount == 0 || edgeQuery.separation > clipSeparation + edgeTol )
|
||||
{
|
||||
B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < hullA->edgeCount );
|
||||
B3_ASSERT( 0 <= edgeQuery.indexB && edgeQuery.indexB < hullB->edgeCount );
|
||||
|
||||
// Edge contact
|
||||
b3LocalManifold edgeManifold = { 0 };
|
||||
b3LocalManifoldPoint edgePoint = { 0 };
|
||||
edgeManifold.points = &edgePoint;
|
||||
|
||||
b3EdgeQuery edgeQuery = {
|
||||
.normal = axisQuery.normal,
|
||||
.indexA = axisQuery.indexA,
|
||||
.indexB = axisQuery.indexB,
|
||||
.separation = axisQuery.separation,
|
||||
};
|
||||
|
||||
b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache );
|
||||
b3SATCache edgeCache = { 0 };
|
||||
b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, &edgeCache );
|
||||
|
||||
// It is possible with speculation to have vertex-vertex collision that is missed by SAT,
|
||||
// so edge contact yields no points. In that case perhaps the face contact has some points.
|
||||
if ( edgeManifold.pointCount == 1 )
|
||||
{
|
||||
// Copy edge manifold out, being careful to preserve manifold point buffer.
|
||||
|
|
@ -2082,12 +2107,30 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
*manifold = edgeManifold;
|
||||
manifold->points = points;
|
||||
manifold->points[0] = edgePoint;
|
||||
*cache = edgeCache;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
// todo this code has gone stale, will be deleted soon
|
||||
|
||||
typedef struct b3FaceQuery
|
||||
{
|
||||
float separation;
|
||||
int faceIndex;
|
||||
int vertexIndex;
|
||||
} b3FaceQuery;
|
||||
|
||||
typedef struct b3EdgeQuery
|
||||
{
|
||||
b3Vec3 normal;
|
||||
float separation;
|
||||
int indexA;
|
||||
int indexB;
|
||||
} b3EdgeQuery;
|
||||
|
||||
// Old non-SIMD version. Keeping this for testing and comparisons
|
||||
|
||||
static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform )
|
||||
|
|
|
|||
|
|
@ -57,7 +57,12 @@
|
|||
#define B3_SIMD_WIDTH 4
|
||||
//#pragma message("B3_SIMD_SSE2")
|
||||
#elif defined( B3_CPU_ARM )
|
||||
// ARMv7 Neon doesn't have divide or sqrt so cannot be used.
|
||||
#if defined( __aarch64__ ) || defined( _M_ARM64 )
|
||||
#define B3_SIMD_NEON
|
||||
#else
|
||||
#define B3_SIMD_NONE
|
||||
#endif
|
||||
#define B3_SIMD_WIDTH 4
|
||||
//#pragma message("B3_SIMD_NEON")
|
||||
#elif defined( B3_CPU_WASM )
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
// Then from that edge, we select the adjacent face that is most anti-parallel to the reference normal.
|
||||
int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex )
|
||||
{
|
||||
B3_ASSERT( 0 <= vertexIndex && vertexIndex < hull->vertexCount );
|
||||
|
||||
const b3HullVertex* vertices = b3GetHullVertices( hull );
|
||||
const b3HullHalfEdge* edges = b3GetHullEdges( hull );
|
||||
const b3Plane* planes = b3GetHullPlanes( hull );
|
||||
|
|
|
|||
|
|
@ -8,28 +8,21 @@
|
|||
|
||||
#define B3_MAX_CLIP_POINTS 64
|
||||
|
||||
typedef struct b3FaceQuery
|
||||
{
|
||||
float separation;
|
||||
int faceIndex;
|
||||
int vertexIndex;
|
||||
} b3FaceQuery;
|
||||
|
||||
typedef struct b3EdgeQuery
|
||||
{
|
||||
b3Vec3 normal;
|
||||
float separation;
|
||||
int indexA;
|
||||
int indexB;
|
||||
} b3EdgeQuery;
|
||||
|
||||
typedef struct b3AxisQuery
|
||||
typedef struct b3SeparatingAxis
|
||||
{
|
||||
b3Vec3 normal;
|
||||
float separation;
|
||||
int indexA;
|
||||
int indexB;
|
||||
b3SeparatingFeature type;
|
||||
} b3SeparatingAxis;
|
||||
|
||||
typedef struct b3AxisQuery
|
||||
{
|
||||
b3SeparatingAxis faceA;
|
||||
b3SeparatingAxis faceB;
|
||||
b3SeparatingAxis edge;
|
||||
b3SeparatingFeature separatedFeature;
|
||||
} b3AxisQuery;
|
||||
|
||||
typedef struct b3ClipVertex
|
||||
|
|
@ -52,7 +45,7 @@ b3FeaturePair b3FlipPair( b3FeaturePair pair );
|
|||
|
||||
int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane );
|
||||
|
||||
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride );
|
||||
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );
|
||||
|
||||
#if B3_ENABLE_VALIDATION
|
||||
bool b3ValidatePolygon( b3ClipVertex* polygon, int count );
|
||||
|
|
@ -66,3 +59,27 @@ static inline uint32_t b3MakeFeatureId( b3FeaturePair pair )
|
|||
return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) |
|
||||
(uint32_t)pair.index2;
|
||||
}
|
||||
|
||||
static inline b3SeparatingAxis b3GetBestAxis( const b3AxisQuery* query )
|
||||
{
|
||||
B3_VALIDATE( query->faceA.type == b3_faceAxisA );
|
||||
B3_VALIDATE( query->edge.type == b3_edgePairAxis );
|
||||
B3_VALIDATE( query->faceB.type == b3_faceAxisB );
|
||||
|
||||
if ( query->faceA.separation > query->faceB.separation )
|
||||
{
|
||||
if ( query->edge.separation > query->faceA.separation )
|
||||
{
|
||||
return query->edge;
|
||||
}
|
||||
|
||||
return query->faceA;
|
||||
}
|
||||
|
||||
if ( query->edge.separation > query->faceB.separation )
|
||||
{
|
||||
return query->edge;
|
||||
}
|
||||
|
||||
return query->faceB;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -608,7 +608,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
|
|||
switch ( shapeB->type )
|
||||
{
|
||||
case b3_capsuleShape:
|
||||
b3CollideCapsuleAndTriangle( manifold, pointCapacity, &shapeB->capsule, vertices, &cache->simplexCache );
|
||||
b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule, &cache->simplexCache );
|
||||
break;
|
||||
|
||||
case b3_hullShape:
|
||||
|
|
@ -619,14 +619,14 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
|
|||
cache->satCache = (b3SATCache){ 0 };
|
||||
}
|
||||
|
||||
b3CollideHullAndTriangle( manifold, pointCapacity, hullB, vertices[0], vertices[1], vertices[2], triangle.flags,
|
||||
b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], triangle.flags, hullB,
|
||||
&cache->satCache, enableSpeculative );
|
||||
context->satCallCount += 1;
|
||||
context->satCacheHitCount += cache->satCache.hit;
|
||||
break;
|
||||
|
||||
case b3_sphereShape:
|
||||
b3CollideSphereAndTriangle( manifold, pointCapacity, &shapeB->sphere, vertices );
|
||||
b3CollideTriangleAndSphere( manifold, pointCapacity, vertices, &shapeB->sphere );
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
|||
11
src/solver.c
11
src/solver.c
|
|
@ -24,6 +24,8 @@
|
|||
#include <stddef.h>
|
||||
#include <stdio.h>
|
||||
|
||||
_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" );
|
||||
|
||||
// these are useful for solver testing
|
||||
#define ITERATIONS 1
|
||||
#define RELAX_ITERATIONS 1
|
||||
|
|
@ -1313,6 +1315,7 @@ static void b3SolverTask( void* taskContext )
|
|||
stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount;
|
||||
|
||||
// Restitution
|
||||
for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration )
|
||||
{
|
||||
b3ApplyRestitution_Overflow( context );
|
||||
|
||||
|
|
@ -1324,7 +1327,7 @@ static void b3SolverTask( void* taskContext )
|
|||
b3ExecuteMainStage( stages + iterStageIndex, context, syncBits );
|
||||
iterStageIndex += 1;
|
||||
}
|
||||
// graphSyncIndex += 1;
|
||||
graphSyncIndex += 1;
|
||||
stageIndex += activeColorCount;
|
||||
}
|
||||
|
||||
|
|
@ -1690,7 +1693,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
// b3_stageRelax
|
||||
stageCount += RELAX_ITERATIONS * activeColorCount;
|
||||
// b3_stageRestitution
|
||||
stageCount += activeColorCount;
|
||||
stageCount += B3_RESTITUTION_ITERATIONS * activeColorCount;
|
||||
// b3_stageStoreWideImpulses
|
||||
stageCount += 1;
|
||||
// b3_stageStoreImpulses
|
||||
|
|
@ -1775,8 +1778,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts,
|
||||
activeColorIndices );
|
||||
// Note: joint blocks mixed in, could have joint limit restitution
|
||||
stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts,
|
||||
activeColorIndices );
|
||||
stage = b3InitColorStages( stage, b3_stageRestitution, B3_RESTITUTION_ITERATIONS, activeColorCount, graphColorBlocks,
|
||||
graphBlockCounts, activeColorIndices );
|
||||
stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX );
|
||||
stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX );
|
||||
|
||||
|
|
|
|||
|
|
@ -23,7 +23,8 @@
|
|||
#define WIN32_LEAN_AND_MEAN 1
|
||||
#endif
|
||||
|
||||
#include <Windows.h>
|
||||
// Lower-case windows.h intentionally for cross compiling on mingw.
|
||||
#include <windows.h>
|
||||
#include <limits.h>
|
||||
|
||||
static double s_invFrequency = 0.0;
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache )
|
|||
return s_triangleFeatures[mask];
|
||||
}
|
||||
|
||||
void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB )
|
||||
void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB )
|
||||
{
|
||||
manifold->pointCount = 0;
|
||||
|
||||
|
|
@ -59,8 +59,8 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
return;
|
||||
}
|
||||
|
||||
b3Vec3 center = sphereA->center;
|
||||
b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2];
|
||||
b3Vec3 center = sphereB->center;
|
||||
b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];
|
||||
b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );
|
||||
|
||||
float offset = b3PlaneSeparation( plane, center );
|
||||
|
|
@ -70,19 +70,23 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
return;
|
||||
}
|
||||
|
||||
float radius = sphereB->radius;
|
||||
|
||||
// Closest point on triangle to sphere center
|
||||
b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center );
|
||||
|
||||
// Test separating axis
|
||||
float squaredDistance = b3DistanceSquared( closest.point, center );
|
||||
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
|
||||
float maxDistance = sphereA->radius + speculativeDistance;
|
||||
float maxDistance = radius + speculativeDistance;
|
||||
if ( squaredDistance > maxDistance * maxDistance )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
float distance = sqrtf( squaredDistance );
|
||||
|
||||
// Normal points from triangle to sphere
|
||||
b3Vec3 normal;
|
||||
if ( distance * distance > 1000.0f * FLT_MIN )
|
||||
{
|
||||
|
|
@ -94,7 +98,8 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
}
|
||||
|
||||
// contact point mid-way
|
||||
b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( sphereA->radius, normal ) ), closest.point ) );
|
||||
// p = 0.5 * (c + q) - 0.5 * r * n
|
||||
b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( radius, normal ) ), closest.point ) );
|
||||
|
||||
manifold->normal = normal;
|
||||
manifold->pointCount = 1;
|
||||
|
|
@ -103,7 +108,7 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
|
||||
b3LocalManifoldPoint* mp = manifold->points + 0;
|
||||
mp->point = contactPoint;
|
||||
mp->separation = distance - sphereA->radius;
|
||||
mp->separation = distance - radius;
|
||||
mp->pair = b3FeaturePair_single;
|
||||
}
|
||||
|
||||
|
|
@ -156,28 +161,30 @@ static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3*
|
|||
return true;
|
||||
}
|
||||
|
||||
static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule )
|
||||
static b3SeparatingAxis b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule )
|
||||
{
|
||||
float separation1 = b3PlaneSeparation( plane, capsule->center1 );
|
||||
float separation2 = b3PlaneSeparation( plane, capsule->center2 );
|
||||
|
||||
if ( separation1 < separation2 )
|
||||
{
|
||||
return (b3FaceQuery){
|
||||
return (b3SeparatingAxis){
|
||||
.normal = plane.normal,
|
||||
.separation = separation1,
|
||||
.faceIndex = 0,
|
||||
.vertexIndex = 0,
|
||||
.indexA = 0,
|
||||
.indexB = 0,
|
||||
};
|
||||
}
|
||||
|
||||
return (b3FaceQuery){
|
||||
return (b3SeparatingAxis){
|
||||
.normal = plane.normal,
|
||||
.separation = separation2,
|
||||
.faceIndex = 0,
|
||||
.vertexIndex = 1,
|
||||
.indexA = 0,
|
||||
.indexB = 1,
|
||||
};
|
||||
}
|
||||
|
||||
static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )
|
||||
static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )
|
||||
{
|
||||
// Work in the local space of the capsule
|
||||
b3Vec3 p1 = capsule->center1;
|
||||
|
|
@ -243,7 +250,7 @@ static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Pla
|
|||
}
|
||||
|
||||
// Save result
|
||||
return (b3EdgeQuery){
|
||||
return (b3SeparatingAxis){
|
||||
.normal = maxNormal,
|
||||
.separation = maxSeparation,
|
||||
.indexA = maxIndex1,
|
||||
|
|
@ -300,7 +307,7 @@ static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, con
|
|||
}
|
||||
|
||||
static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
||||
const b3Capsule* capsule, b3EdgeQuery query )
|
||||
const b3Capsule* capsule, b3SeparatingAxis query )
|
||||
{
|
||||
B3_ASSERT( 0 <= query.indexA && query.indexA < 3 );
|
||||
|
||||
|
|
@ -355,7 +362,7 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con
|
|||
pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB );
|
||||
}
|
||||
|
||||
void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Vec3* triangleB,
|
||||
void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Capsule* capsuleB,
|
||||
b3SimplexCache* cache )
|
||||
{
|
||||
manifold->pointCount = 0;
|
||||
|
|
@ -365,9 +372,9 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
return;
|
||||
}
|
||||
|
||||
b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2];
|
||||
b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];
|
||||
b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );
|
||||
b3Vec3 capsuleCenter = b3Lerp( capsuleA->center1, capsuleA->center2, 0.5f );
|
||||
b3Vec3 capsuleCenter = b3Lerp( capsuleB->center1, capsuleB->center2, 0.5f );
|
||||
|
||||
float offset = b3PlaneSeparation( plane, capsuleCenter );
|
||||
if ( offset < 0.0f )
|
||||
|
|
@ -377,14 +384,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
}
|
||||
|
||||
b3DistanceInput distanceInput;
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f };
|
||||
distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f };
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleA, 3, 0.0f };
|
||||
distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f };
|
||||
distanceInput.transform = b3Transform_identity;
|
||||
distanceInput.useRadii = false;
|
||||
|
||||
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 );
|
||||
|
||||
float radius = capsuleA->radius;
|
||||
float radius = capsuleB->radius;
|
||||
if ( distanceOutput.distance > radius + B3_SPECULATIVE_DISTANCE )
|
||||
{
|
||||
// Shapes are separated, persist the cache
|
||||
|
|
@ -403,14 +410,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
{
|
||||
// Clip capsule segment against side planes of reference face
|
||||
b3ClipVertex segment[2];
|
||||
segment[0].position = capsuleA->center1;
|
||||
segment[0].position = capsuleB->center1;
|
||||
segment[0].separation = 0.0f;
|
||||
segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 );
|
||||
segment[1].position = capsuleA->center2;
|
||||
segment[1].position = capsuleB->center2;
|
||||
segment[1].separation = 0.0f;
|
||||
segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 );
|
||||
|
||||
bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleB, plane );
|
||||
bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleA, plane );
|
||||
|
||||
if ( havePoints == true )
|
||||
{
|
||||
|
|
@ -421,6 +428,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal );
|
||||
b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal );
|
||||
|
||||
// Normal points from triangle to capsule
|
||||
manifold->normal = normal;
|
||||
manifold->feature = b3_featureTriangleFace;
|
||||
manifold->pointCount = 2;
|
||||
|
|
@ -439,9 +447,10 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
}
|
||||
}
|
||||
|
||||
// Create contact from closest points
|
||||
// Create contact from closest points.
|
||||
b3Vec3 point = b3MulSV( 0.5f, b3Add( b3Sub( distanceOutput.pointA, b3MulSV( radius, delta ) ), distanceOutput.pointB ) );
|
||||
|
||||
// Normal points from triangle to capsule.
|
||||
manifold->normal = delta;
|
||||
manifold->pointCount = 1;
|
||||
manifold->feature = b3GetTriangleFeature( cache );
|
||||
|
|
@ -456,14 +465,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
|
||||
// Deep penetration
|
||||
|
||||
b3FaceQuery faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleA );
|
||||
b3SeparatingAxis faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleB );
|
||||
if ( faceQuery.separation > radius )
|
||||
{
|
||||
// Shapes are separated
|
||||
return;
|
||||
}
|
||||
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, plane, capsuleA );
|
||||
b3SeparatingAxis edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleA, plane, capsuleB );
|
||||
if ( edgeQuery.separation > radius )
|
||||
{
|
||||
// Shapes are separated
|
||||
|
|
@ -472,7 +481,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
|
||||
// Create face contact
|
||||
float faceSeparation = faceQuery.separation - radius;
|
||||
b3BuildTriangleAndCapsuleFaceContact( manifold, triangleB, plane, capsuleA );
|
||||
b3BuildTriangleAndCapsuleFaceContact( manifold, triangleA, plane, capsuleB );
|
||||
if ( manifold->pointCount == 2 )
|
||||
{
|
||||
faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation );
|
||||
|
|
@ -487,7 +496,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance )
|
||||
{
|
||||
// Edge contact
|
||||
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, plane, capsuleA, edgeQuery );
|
||||
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleA, plane, capsuleB, edgeQuery );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -512,31 +521,35 @@ static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction )
|
|||
return index;
|
||||
}
|
||||
|
||||
static b3FaceQuery b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
static b3SeparatingAxis b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
{
|
||||
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
||||
b3Plane plane = triangle->plane;
|
||||
int vertexIndex = b3FindHullSupportVertex( hull, b3Neg( plane.normal ) );
|
||||
b3Vec3 normal = b3Neg( plane.normal );
|
||||
int vertexIndex = b3FindHullSupportVertex( hull, normal );
|
||||
b3Vec3 support = hullPoints[vertexIndex];
|
||||
float separation = b3PlaneSeparation( plane, support );
|
||||
|
||||
return (b3FaceQuery){
|
||||
return (b3SeparatingAxis){
|
||||
.normal = plane.normal,
|
||||
.separation = separation,
|
||||
.faceIndex = 0,
|
||||
.vertexIndex = (uint8_t)vertexIndex,
|
||||
.indexA = 0,
|
||||
.indexB = vertexIndex,
|
||||
.type = b3_faceAxisA,
|
||||
};
|
||||
}
|
||||
|
||||
static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
static b3SeparatingAxis b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
{
|
||||
const b3Plane* hullPlanes = b3GetHullPlanes( hull );
|
||||
int faceCount = hull->faceCount;
|
||||
|
||||
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
|
||||
|
||||
int maxFaceIndex = -1;
|
||||
int maxVertexIndex = -1;
|
||||
float maxFaceSeparation = -FLT_MAX;
|
||||
b3Vec3 maxNormal = b3Vec3_zero;
|
||||
float maxFaceSeparation = -INFINITY;
|
||||
int maxFaceIndex = B3_NULL_INDEX;
|
||||
int maxVertexIndex = B3_NULL_INDEX;
|
||||
|
||||
for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex )
|
||||
{
|
||||
|
|
@ -547,26 +560,32 @@ static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3Hull
|
|||
float separation = b3PlaneSeparation( plane, support );
|
||||
if ( separation > maxFaceSeparation )
|
||||
{
|
||||
maxNormal = plane.normal;
|
||||
maxFaceSeparation = separation;
|
||||
maxFaceIndex = faceIndex;
|
||||
maxVertexIndex = vertexIndex;
|
||||
maxFaceSeparation = separation;
|
||||
}
|
||||
}
|
||||
|
||||
return (b3FaceQuery){
|
||||
// Normal points from triangle to hull
|
||||
return (b3SeparatingAxis){
|
||||
.normal = b3Neg( maxNormal ),
|
||||
.separation = maxFaceSeparation,
|
||||
.faceIndex = maxFaceIndex,
|
||||
.vertexIndex = maxVertexIndex,
|
||||
.indexA = maxVertexIndex,
|
||||
.indexB = maxFaceIndex,
|
||||
.type = b3_faceAxisB,
|
||||
};
|
||||
}
|
||||
|
||||
static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
// A: hull, B: triangle
|
||||
static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
{
|
||||
b3EdgeQuery result = {
|
||||
b3SeparatingAxis result = {
|
||||
.normal = b3Vec3_zero,
|
||||
.separation = -FLT_MAX,
|
||||
.separation = -INFINITY,
|
||||
.indexA = B3_NULL_INDEX,
|
||||
.indexB = B3_NULL_INDEX,
|
||||
.type = b3_edgePairAxis,
|
||||
};
|
||||
|
||||
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
|
||||
|
|
@ -645,8 +664,12 @@ static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle,
|
|||
}
|
||||
|
||||
static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,
|
||||
const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative )
|
||||
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )
|
||||
{
|
||||
B3_VALIDATE( query.type == b3_faceAxisB );
|
||||
B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );
|
||||
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->faceCount );
|
||||
|
||||
manifold->pointCount = 0;
|
||||
|
||||
const b3HullFace* hullFaces = b3GetHullFaces( hull );
|
||||
|
|
@ -655,8 +678,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
|
|||
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
||||
|
||||
// Reference hull face
|
||||
int refFace = query.faceIndex;
|
||||
b3Plane refPlane = hullPlanes[refFace];
|
||||
b3Plane refPlane = hullPlanes[query.indexB];
|
||||
|
||||
// Build clip polygon from triangle face (the incident face)
|
||||
b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS];
|
||||
|
|
@ -679,7 +701,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
|
|||
b3ClipVertex* input = buffer1;
|
||||
b3ClipVertex* output = buffer2;
|
||||
|
||||
const b3HullFace* face = hullFaces + refFace;
|
||||
const b3HullFace* face = hullFaces + query.indexB;
|
||||
int edgeIndex = face->edge;
|
||||
|
||||
do
|
||||
|
|
@ -751,14 +773,17 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
|
|||
// Save cache
|
||||
cache->separation = minSeparation;
|
||||
cache->type = b3_faceAxisB;
|
||||
cache->indexA = (uint8_t)query.vertexIndex;
|
||||
cache->indexB = (uint8_t)query.faceIndex;
|
||||
cache->indexA = (uint8_t)query.indexA;
|
||||
cache->indexB = (uint8_t)query.indexB;
|
||||
return minSeparation;
|
||||
}
|
||||
|
||||
static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,
|
||||
const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative )
|
||||
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )
|
||||
{
|
||||
B3_VALIDATE( query.type == b3_faceAxisA );
|
||||
B3_VALIDATE( query.indexA == 0 );
|
||||
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->vertexCount );
|
||||
B3_VALIDATE( manifold->pointCount == 0 );
|
||||
|
||||
const b3HullFace* hullFaces = b3GetHullFaces( hull );
|
||||
|
|
@ -766,10 +791,9 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
|
|||
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
||||
|
||||
// Find incident face
|
||||
B3_ASSERT( query.faceIndex == 0 );
|
||||
b3Plane refPlane = triangle->plane;
|
||||
|
||||
int incFace = b3FindIncidentFace( hull, refPlane.normal, query.vertexIndex );
|
||||
int incFace = b3FindIncidentFace( hull, refPlane.normal, query.indexB );
|
||||
|
||||
// Build clip polygon from incident face
|
||||
b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS];
|
||||
|
|
@ -865,16 +889,18 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
|
|||
// Save cache
|
||||
cache->separation = minSeparation;
|
||||
cache->type = b3_faceAxisA;
|
||||
cache->indexA = (uint8_t)query.faceIndex;
|
||||
cache->indexB = (uint8_t)query.vertexIndex;
|
||||
cache->indexA = (uint8_t)query.indexA;
|
||||
cache->indexB = (uint8_t)query.indexB;
|
||||
return minSeparation;
|
||||
}
|
||||
|
||||
static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,
|
||||
const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache )
|
||||
static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,
|
||||
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache )
|
||||
{
|
||||
B3_VALIDATE( query.type == b3_edgePairAxis );
|
||||
B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );
|
||||
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->edgeCount );
|
||||
B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE );
|
||||
B3_ASSERT( query.indexA < 3 );
|
||||
|
||||
b3Vec3 pA = trianglePoint;
|
||||
b3Vec3 eA = triangleEdge;
|
||||
|
|
@ -927,9 +953,9 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci
|
|||
b3AtomicInt b3_triangleConvexCalls;
|
||||
b3AtomicInt b3_triangleCacheHits;
|
||||
|
||||
// Computes the manifold in the local space of the hull
|
||||
void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3,
|
||||
int triangleFlags, b3SATCache* cache, bool enableSpeculative )
|
||||
// Triangle is in the local space of the hull for efficiency.
|
||||
void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,
|
||||
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative )
|
||||
{
|
||||
manifold->pointCount = 0;
|
||||
manifold->feature = b3_featureNone;
|
||||
|
|
@ -942,7 +968,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 );
|
||||
float linearSlop = B3_LINEAR_SLOP;
|
||||
|
||||
float offset = b3PlaneSeparation( trianglePlane, hullA->center );
|
||||
float offset = b3PlaneSeparation( trianglePlane, hullB->center );
|
||||
if ( cache->type == b3_backsideAxis )
|
||||
{
|
||||
// Use hysteresis to avoid jitter on wavy meshes
|
||||
|
|
@ -976,9 +1002,9 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
.flags = triangleFlags,
|
||||
};
|
||||
|
||||
const b3HullHalfEdge* edges = b3GetHullEdges( hullA );
|
||||
const b3Plane* hullPlanes = b3GetHullPlanes( hullA );
|
||||
const b3Vec3* hullPoints = b3GetHullPoints( hullA );
|
||||
const b3HullHalfEdge* edges = b3GetHullEdges( hullB );
|
||||
const b3Plane* hullPlanes = b3GetHullPlanes( hullB );
|
||||
const b3Vec3* hullPoints = b3GetHullPoints( hullB );
|
||||
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
||||
cache->hit = 1;
|
||||
|
|
@ -990,7 +1016,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
{
|
||||
B3_ASSERT( cache->indexA == 0 );
|
||||
|
||||
int vertexIndex = b3FindHullSupportVertex( hullA, b3Neg( trianglePlane.normal ) );
|
||||
int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( trianglePlane.normal ) );
|
||||
b3Vec3 support = hullPoints[vertexIndex];
|
||||
float separation = b3PlaneSeparation( trianglePlane, support );
|
||||
if ( separation > speculativeDistance )
|
||||
|
|
@ -999,15 +1025,17 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
return;
|
||||
}
|
||||
|
||||
b3FaceQuery faceQuery;
|
||||
b3SeparatingAxis faceQuery;
|
||||
faceQuery.normal = trianglePlane.normal;
|
||||
faceQuery.separation = separation;
|
||||
faceQuery.faceIndex = cache->indexA;
|
||||
faceQuery.vertexIndex = vertexIndex;
|
||||
faceQuery.indexA = cache->indexA;
|
||||
faceQuery.indexB = vertexIndex;
|
||||
faceQuery.type = b3_faceAxisA;
|
||||
|
||||
// Read cache but don't modify it
|
||||
b3SATCache localCache = *cache;
|
||||
float clippedSeparation =
|
||||
b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative );
|
||||
b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );
|
||||
|
||||
if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop )
|
||||
{
|
||||
|
|
@ -1023,7 +1051,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
|
||||
case b3_faceAxisB:
|
||||
{
|
||||
B3_ASSERT( cache->indexB < hullA->faceCount );
|
||||
B3_ASSERT( cache->indexB < hullB->faceCount );
|
||||
|
||||
b3Plane plane = hullPlanes[cache->indexB];
|
||||
|
||||
|
|
@ -1058,15 +1086,17 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
if ( isDeep == false )
|
||||
{
|
||||
// Try to rebuild contact from last features
|
||||
b3FaceQuery faceQuery;
|
||||
b3SeparatingAxis faceQuery;
|
||||
faceQuery.normal = b3Neg( plane.normal );
|
||||
faceQuery.separation = separation;
|
||||
faceQuery.faceIndex = cache->indexB;
|
||||
faceQuery.vertexIndex = vertexIndex;
|
||||
faceQuery.indexA = vertexIndex;
|
||||
faceQuery.indexB = cache->indexB;
|
||||
faceQuery.type = b3_faceAxisB;
|
||||
|
||||
// Read cache but don't modify it
|
||||
b3SATCache localCache = *cache;
|
||||
float clippedSeparation =
|
||||
b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative );
|
||||
b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );
|
||||
|
||||
// Cache reuse is only successful if it creates contact points and the clipped separation didn't change much.
|
||||
if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop )
|
||||
|
|
@ -1090,7 +1120,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
b3Vec3 triPoint = trianglePoints[indexA];
|
||||
b3Vec3 triEdge = triangleEdges[indexA];
|
||||
|
||||
B3_ASSERT( cache->indexB < hullA->edgeCount - 1 );
|
||||
B3_ASSERT( cache->indexB < hullB->edgeCount - 1 );
|
||||
int indexB = cache->indexB;
|
||||
|
||||
const b3HullHalfEdge* edge2 = edges + indexB;
|
||||
|
|
@ -1134,15 +1164,16 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
{
|
||||
// Try to rebuild contact from last features
|
||||
// Flip normal to point from triangle to hull
|
||||
b3EdgeQuery edgeQuery;
|
||||
b3SeparatingAxis edgeQuery;
|
||||
edgeQuery.normal = b3Neg( axis );
|
||||
edgeQuery.indexA = indexA;
|
||||
edgeQuery.indexB = indexB;
|
||||
edgeQuery.separation = separation;
|
||||
edgeQuery.type = b3_edgePairAxis;
|
||||
|
||||
// Read cache but don't modify it
|
||||
b3SATCache localCache = *cache;
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, hullA, edgeQuery, &localCache );
|
||||
b3CollideTriangleAndHullEdges( manifold, capacity, triPoint, triEdge, hullB, edgeQuery, &localCache );
|
||||
|
||||
if ( manifold->pointCount > 0 )
|
||||
{
|
||||
|
|
@ -1161,28 +1192,28 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
// This case is for testing
|
||||
case b3_manualFaceAxisA:
|
||||
{
|
||||
b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA );
|
||||
b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative );
|
||||
b3SeparatingAxis query = b3QueryTriangleFace( &triangle, hullB );
|
||||
b3CollideTriangleFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );
|
||||
return;
|
||||
}
|
||||
|
||||
// This case is for testing
|
||||
case b3_manualFaceAxisB:
|
||||
{
|
||||
b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA );
|
||||
b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative );
|
||||
b3SeparatingAxis query = b3QueryHullFace( &triangle, hullB );
|
||||
b3CollideHullFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );
|
||||
return;
|
||||
}
|
||||
|
||||
// This case is for testing
|
||||
case b3_manualEdgePairAxis:
|
||||
{
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA );
|
||||
if ( edgeQuery.indexA != B3_NULL_INDEX )
|
||||
b3SeparatingAxis query = b3QueryTriangleAndHullEdges( &triangle, hullB );
|
||||
if ( query.indexA != B3_NULL_INDEX )
|
||||
{
|
||||
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
|
||||
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache );
|
||||
b3Vec3 trianglePoint = trianglePoints[query.indexA];
|
||||
b3Vec3 triangleEdge = triangleEdges[query.indexA];
|
||||
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, query, cache );
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
|
@ -1196,29 +1227,29 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
cache->hit = 0;
|
||||
|
||||
// Find axis of minimum penetration
|
||||
b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA );
|
||||
b3SeparatingAxis faceQueryA = b3QueryTriangleFace( &triangle, hullB );
|
||||
if ( faceQueryA.separation > speculativeDistance )
|
||||
{
|
||||
// Separating axis found
|
||||
cache->separation = faceQueryA.separation;
|
||||
cache->type = b3_faceAxisA;
|
||||
cache->indexA = 0;
|
||||
cache->indexB = UINT8_MAX;
|
||||
cache->indexA = (uint8_t)faceQueryA.indexA;
|
||||
cache->indexB = (uint8_t)faceQueryA.indexB;
|
||||
return;
|
||||
}
|
||||
|
||||
b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA );
|
||||
b3SeparatingAxis faceQueryB = b3QueryHullFace( &triangle, hullB );
|
||||
if ( faceQueryB.separation > speculativeDistance )
|
||||
{
|
||||
// Separating axis found
|
||||
cache->separation = faceQueryB.separation;
|
||||
cache->type = b3_faceAxisB;
|
||||
cache->indexA = UINT8_MAX;
|
||||
cache->indexB = (uint8_t)faceQueryB.faceIndex;
|
||||
cache->indexA = (uint8_t)faceQueryB.indexA;
|
||||
cache->indexB = (uint8_t)faceQueryB.indexB;
|
||||
return;
|
||||
}
|
||||
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA );
|
||||
b3SeparatingAxis edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullB );
|
||||
if ( edgeQuery.separation > speculativeDistance )
|
||||
{
|
||||
// Separating axis found
|
||||
|
|
@ -1229,20 +1260,18 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
return;
|
||||
}
|
||||
|
||||
float clippedFaceSeparation;
|
||||
float clipSeparation;
|
||||
|
||||
// Don't admit a hull face significantly opposed to the triangle face.
|
||||
// Need a tolerance to avoid ghost collisions.
|
||||
b3Vec3 hullNormal = hullPlanes[faceQueryB.faceIndex].normal;
|
||||
bool pushingDown = b3Dot( hullNormal, trianglePlane.normal ) > 0.25f;
|
||||
bool pushingDown = b3Dot( faceQueryB.normal, trianglePlane.normal ) < -0.25f;
|
||||
if ( faceQueryB.separation >= faceQueryA.separation && pushingDown == false )
|
||||
{
|
||||
clippedFaceSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative );
|
||||
clipSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative );
|
||||
}
|
||||
else
|
||||
{
|
||||
clippedFaceSeparation =
|
||||
b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative );
|
||||
clipSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQueryA, cache, enableSpeculative );
|
||||
}
|
||||
|
||||
// Does an edge axis exist?
|
||||
|
|
@ -1253,13 +1282,13 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation );
|
||||
|
||||
if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) ||
|
||||
( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) )
|
||||
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) )
|
||||
{
|
||||
B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 );
|
||||
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
|
||||
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
|
||||
manifold->pointCount = 0;
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache );
|
||||
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1274,7 +1303,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
.count = 3,
|
||||
.radius = 0.0f,
|
||||
};
|
||||
input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f };
|
||||
input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullB->vertexCount, .radius = 0.0f };
|
||||
input.transform = b3Transform_identity;
|
||||
input.useRadii = false;
|
||||
|
||||
|
|
@ -1294,5 +1323,8 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
// This feature pair not accurate but maybe it doesn't matter
|
||||
manifold->points[0].pair = b3FeaturePair_single;
|
||||
}
|
||||
|
||||
// No way to cache this scenario
|
||||
*cache = (b3SATCache){ 0 };
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,8 +20,8 @@
|
|||
#if defined( BOX3D_DOUBLE_PRECISION )
|
||||
#define RAGDOLL_SLEEP_STEP 297
|
||||
#define RAGDOLL_HASH 0x27FF38C1
|
||||
#define WAVE_PILE_SLEEP_STEP 258
|
||||
#define WAVE_PILE_HASH 0xA3CDC61B
|
||||
#define WAVE_PILE_SLEEP_STEP 297
|
||||
#define WAVE_PILE_HASH 0x420CA784
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0x1737F5BC
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
|
|
@ -31,8 +31,8 @@
|
|||
#else
|
||||
#define RAGDOLL_SLEEP_STEP 308
|
||||
#define RAGDOLL_HASH 0x1E5EDD79
|
||||
#define WAVE_PILE_SLEEP_STEP 239
|
||||
#define WAVE_PILE_HASH 0xA2F4D472
|
||||
#define WAVE_PILE_SLEEP_STEP 273
|
||||
#define WAVE_PILE_HASH 0x47233541
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0xB9F993A5
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
|
|
|
|||
|
|
@ -420,7 +420,7 @@ static int TriangleEdgeTest( void )
|
|||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { .type = b3_manualEdgePairAxis };
|
||||
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
|
||||
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
|
||||
|
||||
b3Vec3 expectedNormal = b3Neg( axis );
|
||||
b3Vec3 expectedPoint = b3MulAdd( hullPoint, 0.5f * gap, axis );
|
||||
|
|
@ -453,7 +453,7 @@ static int TriangleEdgeTest( void )
|
|||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
|
||||
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
|
||||
|
||||
ENSURE( manifold.pointCount == 0 );
|
||||
ENSURE( cache.type == b3_edgePairAxis );
|
||||
|
|
@ -486,7 +486,7 @@ static int TriangleParallelEdgeTest( void )
|
|||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
|
||||
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
|
||||
|
||||
ENSURE( manifold.pointCount == 4 );
|
||||
ENSURE( cache.type == b3_faceAxisA );
|
||||
|
|
@ -501,72 +501,111 @@ static int TriangleParallelEdgeTest( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Two long roof ridges laid across each other. Unlike the stacked cubes the axis of minimum
|
||||
// penetration really is the edge pair, so the query has to hand the winning pair to the contact
|
||||
// builder. Closing the crossing angle drives that pair toward parallel, where the cross product
|
||||
// the builder needs to orient the contact loses its precision.
|
||||
// A crossed ridge pair must land on a four point roof face contact. The clipped face
|
||||
// separation can be no deeper than root2 times the vertical overlap.
|
||||
static int CheckRoofFaceContact( const b3LocalManifold* manifold, const b3SATCache* cache, float overlap )
|
||||
{
|
||||
ENSURE( manifold->pointCount == 4 );
|
||||
ENSURE( cache->type == b3_faceAxisA || cache->type == b3_faceAxisB );
|
||||
|
||||
// A roof face of one hull, so 45 degrees off the vertical
|
||||
ENSURE_SMALL( manifold->normal.y - kHalfRoot2, 1e-4f );
|
||||
|
||||
float minSeparation = MinSeparation( manifold );
|
||||
ENSURE( minSeparation < -kHalfRoot2 * overlap + 1e-4f );
|
||||
ENSURE( minSeparation > -kRoot2 * overlap - 1e-4f );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Two long roof ridges laid across each other. The axis of minimum penetration is the edge
|
||||
// pair, but a one point edge contact is weak for stacking. The collider builds the roof face
|
||||
// contact first and only switches to the edge contact when the edge axis beats the clipped
|
||||
// face separation by more than the slop. This pins all three regimes of that policy.
|
||||
static int RidgeCrossingTest( void )
|
||||
{
|
||||
b3BoxHull hullA = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) );
|
||||
b3BoxHull hullB = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) );
|
||||
|
||||
float ridgeY = 0.1f * kRoot2;
|
||||
float overlap = 0.01f;
|
||||
float lift = 2.0f * ridgeY - overlap;
|
||||
|
||||
// Well clear of the rejection threshold. The ridges cross over the origin and the axis is y.
|
||||
float crossingAngles[] = { 0.02f, 0.1f, 0.5f };
|
||||
|
||||
for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i )
|
||||
// Shallow overlap. The edge axis is better by only ( root2 - 1 ) * overlap, inside the
|
||||
// slop, so the four point face contact carries the crossing at every angle.
|
||||
{
|
||||
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) };
|
||||
float overlap = 0.01f;
|
||||
float lift = 2.0f * ridgeY - overlap;
|
||||
float crossingAngles[] = { 0.0f, 1e-3f, 0.02f, 0.1f, 0.5f };
|
||||
|
||||
b3LocalManifoldPoint points[8];
|
||||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
|
||||
for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i )
|
||||
{
|
||||
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) };
|
||||
|
||||
ENSURE( manifold.pointCount == 1 );
|
||||
ENSURE( cache.type == b3_edgePairAxis );
|
||||
ENSURE_SMALL( manifold.normal.x, 1e-5f );
|
||||
ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-5f );
|
||||
ENSURE_SMALL( manifold.normal.z, 1e-5f );
|
||||
ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f );
|
||||
ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f );
|
||||
b3LocalManifoldPoint points[8];
|
||||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
|
||||
|
||||
// Where the point lands along the ridges is ill conditioned at a shallow crossing since
|
||||
// the closest point solve divides by the square of the sine of the angle. It only has to
|
||||
// land near the crossing, not at the end of a three meter beam.
|
||||
ENSURE_SMALL( manifold.points[0].point.x, 0.01f );
|
||||
ENSURE_SMALL( manifold.points[0].point.z, 0.01f );
|
||||
if ( CheckRoofFaceContact( &manifold, &cache, overlap ) != 0 )
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Inside the rejection threshold. A one point edge contact off a parallel pair would have a
|
||||
// normal built from noise, so the query drops the pair and the roof faces carry the contact.
|
||||
float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f };
|
||||
|
||||
for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i )
|
||||
// Deep overlap at a clear crossing. The edge axis now beats the clipped face separation
|
||||
// by more than the slop, so the edge contact replaces the face contact.
|
||||
{
|
||||
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) };
|
||||
float overlap = 0.05f;
|
||||
float lift = 2.0f * ridgeY - overlap;
|
||||
float crossingAngles[] = { 0.05f, 0.1f, 0.2f, 0.5f };
|
||||
|
||||
b3LocalManifoldPoint points[8];
|
||||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
|
||||
for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i )
|
||||
{
|
||||
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) };
|
||||
|
||||
ENSURE( manifold.pointCount == 4 );
|
||||
ENSURE( cache.type != b3_edgePairAxis );
|
||||
b3LocalManifoldPoint points[8];
|
||||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
|
||||
|
||||
// A roof face of one hull, so 45 degrees off the vertical
|
||||
ENSURE_SMALL( manifold.normal.x, 1e-4f );
|
||||
ENSURE_SMALL( manifold.normal.y - kHalfRoot2, 1e-4f );
|
||||
ENSURE_SMALL( b3AbsFloat( manifold.normal.z ) - kHalfRoot2, 1e-4f );
|
||||
ENSURE( manifold.pointCount == 1 );
|
||||
ENSURE( cache.type == b3_edgePairAxis );
|
||||
ENSURE_SMALL( manifold.normal.x, 1e-4f );
|
||||
ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-4f );
|
||||
ENSURE_SMALL( manifold.normal.z, 1e-4f );
|
||||
ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f );
|
||||
ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f );
|
||||
|
||||
float minSeparation = MinSeparation( &manifold );
|
||||
ENSURE( minSeparation < 0.0f );
|
||||
ENSURE( minSeparation > -2.0f * overlap );
|
||||
// Only has to land near the crossing, not at the end of a three meter beam
|
||||
ENSURE_SMALL( manifold.points[0].point.x, 0.01f );
|
||||
ENSURE_SMALL( manifold.points[0].point.z, 0.01f );
|
||||
}
|
||||
}
|
||||
|
||||
// Deep overlap near parallel. A one point edge contact off a parallel pair would have a
|
||||
// normal built from noise, so the roof faces keep the contact.
|
||||
{
|
||||
float overlap = 0.05f;
|
||||
float lift = 2.0f * ridgeY - overlap;
|
||||
float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f };
|
||||
|
||||
for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i )
|
||||
{
|
||||
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) };
|
||||
|
||||
b3LocalManifoldPoint points[8];
|
||||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { 0 };
|
||||
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
|
||||
|
||||
if ( CheckRoofFaceContact( &manifold, &cache, overlap ) != 0 )
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
@ -841,7 +880,7 @@ static int TriangleHullEdgeSweepTest( void )
|
|||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SATCache cache = { .type = b3_manualEdgePairAxis };
|
||||
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
|
||||
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
|
||||
|
||||
if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 )
|
||||
{
|
||||
|
|
@ -912,7 +951,7 @@ static int CapsuleTriangleEdgeDeepTest( void )
|
|||
b3LocalManifold manifold = { 0 };
|
||||
manifold.points = points;
|
||||
b3SimplexCache cache = { 0 };
|
||||
b3CollideCapsuleAndTriangle( &manifold, 8, &capsule, triangle, &cache );
|
||||
b3CollideTriangleAndCapsule( &manifold, 8, triangle, &capsule, &cache );
|
||||
|
||||
// Only the edge contacts exercise the new axis. Face contacts are handled elsewhere.
|
||||
if ( manifold.pointCount != 1 || manifold.feature < b3_featureEdge1 || manifold.feature > b3_featureEdge3 )
|
||||
|
|
|
|||
|
|
@ -124,7 +124,8 @@ static int FaceAxisASeparatedTest( void )
|
|||
b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
|
||||
b3Transform xfB = { { 1.2f, 0.0f, 0.0f }, b3Quat_identity };
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisA );
|
||||
ENSURE_SMALL( q.separation - 0.2f, 1e-5f );
|
||||
|
|
@ -155,7 +156,8 @@ static int FaceAxisBSeparatedTest( void )
|
|||
float d = aExtent + 0.5f + gap;
|
||||
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
|
||||
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisB );
|
||||
ENSURE_SMALL( q.separation - gap, 1e-5f );
|
||||
|
|
@ -181,7 +183,8 @@ static int FaceFarSeparatedTest( void )
|
|||
b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
|
||||
b3Transform xfB = { { 3.0f, 0.0f, 0.0f }, b3Quat_identity };
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisA );
|
||||
ENSURE_SMALL( q.separation - 2.0f, 1e-5f );
|
||||
|
|
@ -209,7 +212,8 @@ static int OffsetFaceAxisBTest( void )
|
|||
float d = aExtent + 0.5f + gap;
|
||||
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
|
||||
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisB );
|
||||
ENSURE_SMALL( q.separation - gap, 1e-4f );
|
||||
|
|
@ -248,7 +252,8 @@ static int EdgePairSweepTest( void )
|
|||
float expected = d - kRoot2;
|
||||
|
||||
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
ENSURE( q.type == b3_edgePairAxis );
|
||||
ENSURE_SMALL( q.separation - expected, 1e-4f );
|
||||
|
|
@ -327,7 +332,8 @@ static int SeparatingAxisOracleTest( void )
|
|||
|
||||
b3Transform xfB = { b3MulSV( reach, NextDirection() ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) };
|
||||
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
|
|
@ -423,7 +429,8 @@ static int OffsetHullOracleTest( void )
|
|||
xfB = (b3Transform){ b3Sub( b3MulSV( reach, dir ), b3RotateVector( qB, offset ) ), qB };
|
||||
}
|
||||
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
|
||||
b3SeparatingAxis q = b3GetBestAxis( &aq );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
|
|
|
|||
Loading…
Reference in a new issue