parent
ef8ef0187a
commit
aaa795e34f
13 changed files with 1416 additions and 300 deletions
|
|
@ -590,7 +590,7 @@ public:
|
|||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { -m_walkRange, m_bodyHalfHeight + 0.1f, 0.0f };
|
||||
bodyDef.position = { -m_walkRangeX, m_bodyHalfHeight + 0.1f, 0.0f };
|
||||
bodyDef.motionLocks.angularX = true;
|
||||
bodyDef.motionLocks.angularY = true;
|
||||
bodyDef.motionLocks.angularZ = true;
|
||||
|
|
@ -613,8 +613,10 @@ public:
|
|||
b3CreateHullShape( m_characterId, &shapeDef, &box.base );
|
||||
}
|
||||
|
||||
m_walkDirection = 1.0f;
|
||||
m_walkSpeed = 350.0f * SRC; // s&box run speed
|
||||
m_walkDirectionX = 1.0f;
|
||||
m_walkDirectionZ = 1.0f;
|
||||
m_walkSpeedX = 350.0f * SRC; // s&box run speed
|
||||
m_walkSpeedZ = 20.0f * SRC;
|
||||
m_launchCount = 0;
|
||||
m_maxLaunchSpeed = 0.0f;
|
||||
m_launchMarkerCount = 0;
|
||||
|
|
@ -850,18 +852,27 @@ public:
|
|||
// Drive the character with pure velocity control: keep the solver's vertical velocity,
|
||||
// set the horizontal velocity. This is how the s&box player controller moves.
|
||||
b3Pos position = b3Body_GetPosition( m_characterId );
|
||||
if ( position.x > m_walkRange )
|
||||
if ( position.x > m_walkRangeX )
|
||||
{
|
||||
m_walkDirection = -1.0f;
|
||||
m_walkDirectionX = -1.0f;
|
||||
}
|
||||
else if ( position.x < -m_walkRange )
|
||||
else if ( position.x < -m_walkRangeX )
|
||||
{
|
||||
m_walkDirection = 1.0f;
|
||||
m_walkDirectionX = 1.0f;
|
||||
}
|
||||
|
||||
if ( position.z > m_walkRangeZ )
|
||||
{
|
||||
m_walkDirectionZ = -1.0f;
|
||||
}
|
||||
else if ( position.z < -m_walkRangeZ )
|
||||
{
|
||||
m_walkDirectionZ= 1.0f;
|
||||
}
|
||||
|
||||
b3Vec3 velocity = b3Body_GetLinearVelocity( m_characterId );
|
||||
velocity.x = m_walkDirection * m_walkSpeed;
|
||||
velocity.z = 0.0f;
|
||||
velocity.x = m_walkDirectionX * m_walkSpeedX;
|
||||
velocity.z = m_walkDirectionZ * m_walkSpeedZ;
|
||||
b3Body_SetLinearVelocity( m_characterId, velocity );
|
||||
|
||||
Sample::Step();
|
||||
|
|
@ -905,10 +916,16 @@ public:
|
|||
|
||||
bool DrawControls() override
|
||||
{
|
||||
float speedU = m_walkSpeed / SRC;
|
||||
if ( ImGui::SliderFloat( "Walk Speed (inch/s)", &speedU, 100.0f, 400.0f, "%.0f" ) )
|
||||
float speedUX = m_walkSpeedX / SRC;
|
||||
if ( ImGui::SliderFloat( "Walk Speed X (inch/s)", &speedUX, 100.0f, 400.0f, "%.0f" ) )
|
||||
{
|
||||
m_walkSpeed = speedU * SRC;
|
||||
m_walkSpeedX = speedUX * SRC;
|
||||
}
|
||||
|
||||
float speedUZ = m_walkSpeedZ / SRC;
|
||||
if ( ImGui::SliderFloat( "Walk Speed Z (inch/s)", &speedUZ, 10.0f, 100.0f, "%.0f" ) )
|
||||
{
|
||||
m_walkSpeedZ = speedUZ * SRC;
|
||||
}
|
||||
|
||||
if ( ImGui::Button( "Reset Counters" ) )
|
||||
|
|
@ -927,14 +944,17 @@ public:
|
|||
return new SBoxGhostCollisions( context );
|
||||
}
|
||||
|
||||
static constexpr float m_walkRange = 3.5f; // turn around beyond +/- this x (meters)
|
||||
static constexpr float m_walkRangeX = 3.5f; // turn around beyond +/- this x (meters)
|
||||
static constexpr float m_walkRangeZ = 0.5f; // turn around beyond +/- this x (meters)
|
||||
static constexpr float m_launchThreshold = 0.5f; // upward m/s counted as a ghost launch (~20 inch/s)
|
||||
static constexpr int m_markerCapacity = 64;
|
||||
|
||||
b3MeshData* m_chunkMesh[2] = {};
|
||||
b3BodyId m_characterId;
|
||||
float m_walkDirection;
|
||||
float m_walkSpeed;
|
||||
float m_walkDirectionX;
|
||||
float m_walkDirectionZ;
|
||||
float m_walkSpeedX;
|
||||
float m_walkSpeedZ;
|
||||
int m_launchCount;
|
||||
float m_maxLaunchSpeed;
|
||||
bool m_wasLaunched;
|
||||
|
|
|
|||
|
|
@ -244,7 +244,7 @@ public:
|
|||
DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) );
|
||||
}
|
||||
|
||||
DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation );
|
||||
DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation );
|
||||
|
||||
b3FeaturePair pair = manifoldPoint.pair;
|
||||
DrawString3D( point + 0.025f * normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1,
|
||||
|
|
@ -572,7 +572,7 @@ public:
|
|||
m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Pos_zero );
|
||||
}
|
||||
|
||||
m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.5f };
|
||||
m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.15f };
|
||||
m_hull = b3MakeBoxHull( 1.0f, 0.5f, 0.5f );
|
||||
|
||||
m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity };
|
||||
|
|
@ -636,17 +636,13 @@ public:
|
|||
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero );
|
||||
}
|
||||
|
||||
m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f };
|
||||
m_capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.05f };
|
||||
m_triangle[0] = { -4.0f, 0.0f, -4.0f };
|
||||
m_triangle[1] = { -4.0f, 0.0f, 0.0f };
|
||||
m_triangle[2] = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
m_transformA = b3WorldTransform_identity;
|
||||
m_transformB = { { 0.0f, 0.5f, 0.0f }, b3Quat_identity };
|
||||
m_transformB.q = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.5f * B3_PI );
|
||||
|
||||
m_transformB = { { -0.500000000, 0.123778239, -0.500000000 },
|
||||
{ { -0.157559350, 0.294042289, 0.821513653 }, -0.462417006 } };
|
||||
m_transformB = { { -1.0f, 0.0f, -1.0f }, b3Quat_identity };
|
||||
}
|
||||
|
||||
void Render() override
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
Generated by sokol-shdc (https://github.com/floooh/sokol-tools)
|
||||
|
||||
Cmdline:
|
||||
sokol-shdc --input shapes/capsule.glsl --output generated/capsule.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
sokol-shdc --input E:/box/box3d/samples/shaders/shapes/capsule.glsl --output E:/box/box3d/samples/shaders/generated/capsule.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
|
||||
Overview:
|
||||
=========
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
Generated by sokol-shdc (https://github.com/floooh/sokol-tools)
|
||||
|
||||
Cmdline:
|
||||
sokol-shdc --input shapes/cube.glsl --output generated/cube.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
sokol-shdc --input E:/box/box3d/samples/shaders/shapes/cube.glsl --output E:/box/box3d/samples/shaders/generated/cube.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
|
||||
Overview:
|
||||
=========
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
Generated by sokol-shdc (https://github.com/floooh/sokol-tools)
|
||||
|
||||
Cmdline:
|
||||
sokol-shdc --input shapes/geom.glsl --output generated/geom.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
sokol-shdc --input E:/box/box3d/samples/shaders/shapes/geom.glsl --output E:/box/box3d/samples/shaders/generated/geom.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
|
||||
Overview:
|
||||
=========
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
Generated by sokol-shdc (https://github.com/floooh/sokol-tools)
|
||||
|
||||
Cmdline:
|
||||
sokol-shdc --input shapes/sphere.glsl --output generated/sphere.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
sokol-shdc --input E:/box/box3d/samples/shaders/shapes/sphere.glsl --output E:/box/box3d/samples/shaders/generated/sphere.glsl.h --slang hlsl5:metal_macos:glsl430
|
||||
|
||||
Overview:
|
||||
=========
|
||||
|
|
|
|||
|
|
@ -12,31 +12,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
static inline bool b3IsMinkowskiFaceIsolated( b3Vec3 a, b3Vec3 b, b3Vec3 n )
|
||||
{
|
||||
// An isolated edge (e.g. like in a capsule) defines a circle through the
|
||||
// origin on the Gauss map. So testing for overlap between this circle and
|
||||
// the arc AB simplifies to a simple plane test.
|
||||
float an = b3Dot( a, n );
|
||||
float bn = b3Dot( b, n );
|
||||
|
||||
return an * bn <= 0.0f;
|
||||
}
|
||||
|
||||
// bxa = cross(b, a) and dxc = cross(d, c)
|
||||
// but in practice we use the edge vector between the faces for robustness
|
||||
static inline bool b3IsMinkowskiFace( b3Vec3 a, b3Vec3 b, b3Vec3 bxa, b3Vec3 c, b3Vec3 d, b3Vec3 dxc )
|
||||
{
|
||||
// Two edges build a face on the Minkowski sum if the associated arcs ab and cd intersect on the Gauss map.
|
||||
// The associated arcs are defined by the adjacent face normals of each edge.
|
||||
float cba = b3Dot( c, bxa );
|
||||
float dba = b3Dot( d, bxa );
|
||||
float adc = b3Dot( a, dxc );
|
||||
float bdc = b3Dot( b, dxc );
|
||||
|
||||
return cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f;
|
||||
}
|
||||
|
||||
static int b3ClipSegment( b3ClipVertex segment[2], b3Plane plane )
|
||||
{
|
||||
int vertexCount = 0;
|
||||
|
|
@ -178,18 +153,20 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c
|
|||
b3Transform capsuleTransform )
|
||||
{
|
||||
// Find axis of minimum penetration
|
||||
b3Vec3 maxNormal = b3Vec3_zero;
|
||||
float maxSeparation = -FLT_MAX;
|
||||
int maxIndex1 = -1;
|
||||
int maxIndex2 = -1;
|
||||
int maxIndexA = B3_NULL_INDEX;
|
||||
int maxIndexB = B3_NULL_INDEX;
|
||||
|
||||
// We perform all computations in local space of the hull
|
||||
b3Vec3 p1 = b3TransformPoint( capsuleTransform, capsule->center1 );
|
||||
b3Vec3 q1 = b3TransformPoint( capsuleTransform, capsule->center2 );
|
||||
b3Vec3 e1 = b3Sub( q1, p1 );
|
||||
b3Vec3 pA = b3TransformPoint( capsuleTransform, capsule->center1 );
|
||||
b3Vec3 qA = b3TransformPoint( capsuleTransform, capsule->center2 );
|
||||
b3Vec3 eA = b3Sub( qA, pA );
|
||||
|
||||
const b3HullHalfEdge* edges = b3GetHullEdges( hull );
|
||||
const b3Vec3* points = b3GetHullPoints( hull );
|
||||
const b3Plane* planes = b3GetHullPlanes( hull );
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
|
||||
for ( int index = 0; index < hull->edgeCount; index += 2 )
|
||||
{
|
||||
|
|
@ -197,42 +174,70 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c
|
|||
const b3HullHalfEdge* twin = edges + index + 1;
|
||||
B3_ASSERT( edge->twin == index + 1 && twin->twin == index );
|
||||
|
||||
b3Vec3 p2 = points[edge->origin];
|
||||
b3Vec3 q2 = points[twin->origin];
|
||||
b3Vec3 e2 = b3Sub( q2, p2 );
|
||||
b3Vec3 qB = points[twin->origin];
|
||||
b3Vec3 uB = planes[edge->face].normal;
|
||||
b3Vec3 vB = planes[twin->face].normal;
|
||||
|
||||
b3Vec3 u2 = planes[edge->face].normal;
|
||||
b3Vec3 v2 = planes[twin->face].normal;
|
||||
// An isolated edge (e.g. like in a capsule) defines a circle through the
|
||||
// origin on the Gauss map. So testing for overlap between this circle and
|
||||
// the arc AB simplifies to a plane test.
|
||||
float cba = b3Dot( uB, eA );
|
||||
float dba = b3Dot( vB, eA );
|
||||
|
||||
if ( b3IsMinkowskiFaceIsolated( u2, v2, e1 ) )
|
||||
if ( cba * dba < 0.0f )
|
||||
{
|
||||
// We can pass any point on the edge and choose
|
||||
// the edge centers for better numerical precision.
|
||||
b3Vec3 c1 = b3MulSV( 0.5f, b3Add( q1, p1 ) );
|
||||
b3Vec3 c2 = hull->center;
|
||||
float separation = b3EdgeEdgeSeparation( q1, e1, c1, q2, e2, c2 );
|
||||
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
||||
if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// The intersection of the arcs on the Gauss map is the edge pair axis. Cast the
|
||||
// arc of hull B (from uB to vB) against the plane containing the arc of hull A:
|
||||
// dot(uB + t * (vB - uB), eA) == 0
|
||||
// then
|
||||
// t = cba / (cba - dba)
|
||||
//
|
||||
// The signs of cba and dba differ (Minkowski test), so the division is safe.
|
||||
//
|
||||
// The axis generated points from B to A by construction since it lands between
|
||||
// two face normals on B. This removes the need to orient the separation axis
|
||||
// using the hull centers.
|
||||
//
|
||||
// The axis is perpendicular to both edges so I can use qA and qB as arbitrary
|
||||
// points on edgeA and edgeB to measure the separation.
|
||||
|
||||
float t = cba / ( cba - dba );
|
||||
b3Vec3 axis = b3Lerp( uB, vB, t );
|
||||
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
||||
axis = b3Normalize( axis );
|
||||
float separation = b3Dot( axis, b3Sub( qA, qB ) );
|
||||
|
||||
if ( separation > maxSeparation )
|
||||
{
|
||||
// Note: We don't exit early if we find a separating axis here since we want to
|
||||
// find the best one for caching and account for the convex radius later.
|
||||
maxNormal = axis;
|
||||
maxSeparation = separation;
|
||||
maxIndex1 = 0;
|
||||
maxIndex2 = index;
|
||||
maxIndexA = 0;
|
||||
maxIndexB = index;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Save result
|
||||
return (b3EdgeQuery){
|
||||
.normal = maxNormal,
|
||||
.separation = maxSeparation,
|
||||
.indexA = (uint8_t)maxIndex1,
|
||||
.indexB = (uint8_t)maxIndex2,
|
||||
.indexA = maxIndexA,
|
||||
.indexB = maxIndexB,
|
||||
};
|
||||
}
|
||||
|
||||
static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA )
|
||||
{
|
||||
// Find axis of minimum penetration
|
||||
b3Vec3 maxNormal = b3Vec3_zero;
|
||||
float maxSeparation = -FLT_MAX;
|
||||
int maxIndexA = B3_NULL_INDEX;
|
||||
int maxIndexB = B3_NULL_INDEX;
|
||||
|
|
@ -247,6 +252,8 @@ static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullD
|
|||
// Work in frame A
|
||||
b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q );
|
||||
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
|
||||
// Arranged to minimize transform operations
|
||||
for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 )
|
||||
{
|
||||
|
|
@ -272,27 +279,53 @@ static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullD
|
|||
b3Vec3 uA = planesA[edgeA->face].normal;
|
||||
b3Vec3 vA = planesA[twinA->face].normal;
|
||||
|
||||
bool isMinkowski;
|
||||
{
|
||||
// Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map.
|
||||
// The associated arcs are defined by the adjacent face normals of each edge.
|
||||
float cba = b3Dot( uB, eA );
|
||||
float dba = b3Dot( vB, eA );
|
||||
float adc = -b3Dot( uA, eB );
|
||||
float bdc = -b3Dot( vA, eB );
|
||||
// See "Collision Detection of Convex Polyhedra Based on Duality Transformation"
|
||||
// Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map.
|
||||
// The associated arcs are defined by the adjacent face normals of each edge.
|
||||
|
||||
isMinkowski = cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f;
|
||||
}
|
||||
// These are signed volumes with an edge optimization to avoid cross products
|
||||
// eA parallel to cross(vA, uA)
|
||||
// eB parallel to cross(vB, uB)
|
||||
// Since only signs are tested, length doesn't matter.
|
||||
|
||||
if ( isMinkowski )
|
||||
float cba = b3Dot( uB, eA );
|
||||
float dba = b3Dot( vB, eA );
|
||||
float adc = -b3Dot( uA, eB );
|
||||
float bdc = -b3Dot( vA, eB );
|
||||
|
||||
if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f )
|
||||
{
|
||||
b3Vec3 centerA = hullA->center;
|
||||
b3Vec3 centerB = b3TransformPoint( transformBtoA, hullB->center );
|
||||
float separation = b3EdgeEdgeSeparation( qA, eA, centerA, qB, eB, centerB );
|
||||
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
||||
if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// The intersection of the arcs on the Gauss map is the edge pair axis. Cast the
|
||||
// arc of hull B (from uB to vB) against the plane containing the arc of hull A:
|
||||
// dot(uB + t * (vB - uB), eA) == 0
|
||||
// then
|
||||
// t = cba / (cba - dba)
|
||||
//
|
||||
// The signs of cba and dba differ (Minkowski test), so the division is safe.
|
||||
//
|
||||
// The axis generated points from B to A by construction since it lands between
|
||||
// two face normals on B. This removes the need to orient the separation axis
|
||||
// using the hull centers.
|
||||
//
|
||||
// The axis is perpendicular to both edges so I can use qA and qB as arbitrary
|
||||
// points on edgeA and edgeB to measure the separation.
|
||||
float t = cba / ( cba - dba );
|
||||
b3Vec3 axis = b3Lerp( uB, vB, t );
|
||||
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
||||
axis = b3Normalize( axis );
|
||||
float separation = b3Dot( axis, b3Sub( qA, qB ) );
|
||||
|
||||
if ( separation > maxSeparation )
|
||||
{
|
||||
// Continues to find the maximum separating axis
|
||||
// Flip normal so it points from A to B
|
||||
maxNormal = b3Neg( axis );
|
||||
maxSeparation = separation;
|
||||
maxIndexA = indexA;
|
||||
maxIndexB = indexB;
|
||||
|
|
@ -302,6 +335,7 @@ static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullD
|
|||
}
|
||||
|
||||
return (b3EdgeQuery){
|
||||
.normal = maxNormal,
|
||||
.separation = maxSeparation,
|
||||
.indexA = maxIndexA,
|
||||
.indexB = maxIndexB,
|
||||
|
|
@ -895,19 +929,11 @@ static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int cap
|
|||
|
||||
const b3HullHalfEdge* edge2 = edges + query.indexB;
|
||||
const b3HullHalfEdge* twin2 = edges + edge2->twin;
|
||||
b3Vec3 ch = hullA->center;
|
||||
b3Vec3 ph = points[edge2->origin];
|
||||
b3Vec3 qh = points[twin2->origin];
|
||||
b3Vec3 eh = b3Sub( qh, ph );
|
||||
|
||||
b3Vec3 normal = b3Cross( ec, eh );
|
||||
normal = b3Normalize( normal );
|
||||
|
||||
// Normal should point outward from hull
|
||||
if ( b3Dot( normal, b3Sub( ph, ch ) ) < 0.0f )
|
||||
{
|
||||
normal = b3Neg( normal );
|
||||
}
|
||||
b3Vec3 normal = query.normal;
|
||||
|
||||
b3SegmentDistanceResult result = b3LineDistance( ph, eh, pc, ec );
|
||||
|
||||
|
|
@ -918,7 +944,6 @@ static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int cap
|
|||
}
|
||||
|
||||
b3Vec3 point = b3MulSV( 0.5f, b3Add( b3MulSub( result.point1, capsuleB->radius, normal ), result.point2 ) );
|
||||
|
||||
float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) );
|
||||
B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP );
|
||||
|
||||
|
|
@ -1235,8 +1260,8 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const
|
|||
return true;
|
||||
}
|
||||
|
||||
static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA,
|
||||
b3EdgeQuery query, b3SATCache* cache )
|
||||
static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3EdgeQuery query, b3SATCache* cache )
|
||||
{
|
||||
// Work in shapeA coordinates
|
||||
const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA );
|
||||
|
|
@ -1249,7 +1274,6 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul
|
|||
|
||||
const b3HullHalfEdge* edgeA = edgesA + query.indexA;
|
||||
const b3HullHalfEdge* twinA = edgesA + edgeA->twin;
|
||||
b3Vec3 centerA = hullA->center;
|
||||
b3Vec3 pA = pointsA[edgeA->origin];
|
||||
b3Vec3 qA = pointsA[twinA->origin];
|
||||
b3Vec3 eA = b3Sub( qA, pA );
|
||||
|
|
@ -1260,14 +1284,7 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul
|
|||
b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twinB->origin] );
|
||||
b3Vec3 eB = b3Sub( qB, pB );
|
||||
|
||||
b3Vec3 normal = b3Cross( eA, eB );
|
||||
normal = b3Normalize( normal );
|
||||
|
||||
if ( b3Dot( normal, b3Sub( pA, centerA ) ) < 0.0f )
|
||||
{
|
||||
normal = b3Neg( normal );
|
||||
}
|
||||
|
||||
b3Vec3 normal = query.normal;
|
||||
b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB );
|
||||
|
||||
if ( b3IsWithinSegments( &result ) == false )
|
||||
|
|
@ -1278,10 +1295,6 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul
|
|||
|
||||
// This can slide off the end from caching
|
||||
float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) );
|
||||
|
||||
// todo I suspect this could trip if the cache becomes invalid
|
||||
// B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP );
|
||||
|
||||
b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) );
|
||||
|
||||
// Result in frame A
|
||||
|
|
@ -1302,8 +1315,8 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul
|
|||
return true;
|
||||
}
|
||||
|
||||
void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA,
|
||||
b3SATCache* cache )
|
||||
void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3SATCache* cache )
|
||||
{
|
||||
manifold->pointCount = 0;
|
||||
|
||||
|
|
@ -1405,55 +1418,64 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
|
|||
|
||||
case b3_edgePairAxis:
|
||||
{
|
||||
int index1 = cache->indexA;
|
||||
const b3HullHalfEdge* edge1 = edgesA + index1;
|
||||
const b3HullHalfEdge* twin1 = edgesA + index1 + 1;
|
||||
B3_ASSERT( edge1->twin == index1 + 1 && twin1->twin == index1 );
|
||||
int indexA = cache->indexA;
|
||||
const b3HullHalfEdge* edge1 = edgesA + indexA;
|
||||
const b3HullHalfEdge* twin1 = edgesA + indexA + 1;
|
||||
B3_ASSERT( edge1->twin == indexA + 1 && twin1->twin == indexA );
|
||||
|
||||
b3Vec3 p1 = pointsA[edge1->origin];
|
||||
b3Vec3 q1 = pointsA[twin1->origin];
|
||||
b3Vec3 e1 = b3Sub( q1, p1 );
|
||||
b3Vec3 pA = pointsA[edge1->origin];
|
||||
b3Vec3 qA = pointsA[twin1->origin];
|
||||
b3Vec3 eA = b3Sub( qA, pA );
|
||||
|
||||
b3Vec3 u1 = planesA[edge1->face].normal;
|
||||
b3Vec3 v1 = planesA[twin1->face].normal;
|
||||
b3Vec3 uA = planesA[edge1->face].normal;
|
||||
b3Vec3 vA = planesA[twin1->face].normal;
|
||||
|
||||
int index2 = cache->indexB;
|
||||
const b3HullHalfEdge* edge2 = edgesB + index2;
|
||||
const b3HullHalfEdge* twin2 = edgesB + index2 + 1;
|
||||
B3_ASSERT( edge2->twin == index2 + 1 && twin2->twin == index2 );
|
||||
int indexB = cache->indexB;
|
||||
const b3HullHalfEdge* edge2 = edgesB + indexB;
|
||||
const b3HullHalfEdge* twin2 = edgesB + indexB + 1;
|
||||
B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB );
|
||||
|
||||
b3Vec3 p2 = b3TransformPoint( transformBtoA, pointsB[edge2->origin] );
|
||||
b3Vec3 q2 = b3TransformPoint( transformBtoA, pointsB[twin2->origin] );
|
||||
b3Vec3 e2 = b3Sub( q2, p2 );
|
||||
b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edge2->origin] );
|
||||
b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twin2->origin] );
|
||||
b3Vec3 eB = b3Sub( qB, pB );
|
||||
|
||||
b3Vec3 u2 = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal );
|
||||
b3Vec3 v2 = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal );
|
||||
b3Vec3 uB = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal );
|
||||
b3Vec3 vB = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal );
|
||||
|
||||
// flipping the signs of u2 and v2
|
||||
// cross(v2, u2) == cross(-v2, -u2)
|
||||
// so we still use -e2
|
||||
// but we can also use e1 = cross(u1, v1) and e2 = cross(u2, v2)
|
||||
bool isMinkowski = b3IsMinkowskiFace( u1, v1, e1, b3Neg( u2 ), b3Neg( v2 ), e2 );
|
||||
if ( isMinkowski == true )
|
||||
float cba = b3Dot( uB, eA );
|
||||
float dba = b3Dot( vB, eA );
|
||||
float adc = -b3Dot( uA, eB );
|
||||
float bdc = -b3Dot( vA, eB );
|
||||
|
||||
if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f )
|
||||
{
|
||||
// Transform reference center of the first hull into local space of the second hull
|
||||
b3Vec3 c1 = hullA->center;
|
||||
b3Vec3 c2 = b3TransformPoint( transformBtoA, hullB->center );
|
||||
|
||||
float separation = b3EdgeEdgeSeparation( p1, e1, c1, p2, e2, c2 );
|
||||
if ( separation > speculativeDistance )
|
||||
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
if ( b3MaxFloat( cba * cba, dba * dba ) >= squaredTolerance * b3LengthSquared( eA ) )
|
||||
{
|
||||
// Cache hit, shapes are separated
|
||||
return;
|
||||
}
|
||||
// Transform reference center of the first hull into local space of the second hull
|
||||
float t = cba / ( cba - dba );
|
||||
b3Vec3 axis = b3Lerp( uB, vB, t );
|
||||
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
||||
axis = b3Normalize( axis );
|
||||
float separation = b3Dot( axis, b3Sub( qA, qB ) );
|
||||
|
||||
if ( separation > speculativeDistance )
|
||||
{
|
||||
// Cache hit, shapes are separated
|
||||
return;
|
||||
}
|
||||
|
||||
// if ( cache->separation <= speculativeDistance )
|
||||
{
|
||||
// Try to rebuild contact from last features
|
||||
b3EdgeQuery edgeQuery;
|
||||
b3EdgeQuery edgeQuery = { 0 };
|
||||
edgeQuery.normal = b3Neg( axis );
|
||||
edgeQuery.separation = 0.0f;
|
||||
edgeQuery.indexA = cache->indexA;
|
||||
edgeQuery.indexB = cache->indexB;
|
||||
edgeQuery.separation = 0.0f;
|
||||
|
||||
b3SATCache localCache = { 0 };
|
||||
bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache );
|
||||
|
|
|
|||
|
|
@ -10,58 +10,6 @@
|
|||
|
||||
#include "box3d/math_functions.h"
|
||||
|
||||
// p1 : origin on edge 1
|
||||
// e1 : edge 1
|
||||
// c1 : shape 1 centroid
|
||||
// p2 : origin on edge 2
|
||||
// e2 : edge 2
|
||||
// c2 : shape 2 centroid
|
||||
float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 )
|
||||
{
|
||||
// Build search direction
|
||||
b3Vec3 u = b3Cross( e1, e2 );
|
||||
float length = b3Length( u );
|
||||
|
||||
// Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2|
|
||||
const float kTolerance = 0.005f;
|
||||
if ( length < kTolerance * sqrtf( b3LengthSquared( e1 ) * b3LengthSquared( e2 ) ) )
|
||||
{
|
||||
return -FLT_MAX;
|
||||
}
|
||||
|
||||
if ( length * length < 1000.0f * FLT_MIN )
|
||||
{
|
||||
return -FLT_MAX;
|
||||
}
|
||||
|
||||
b3Vec3 n = b3MulSV( 1.0f / length, u );
|
||||
|
||||
// Make sure normal points away from the first shape
|
||||
// For a triangle, it is possible that N is aligned with the triangle normal and the sign
|
||||
// value can be close to zero and flicker between small negative and positive values, leading to
|
||||
// an incorrect separation value. So we assume the other hull has some volume and pick the most
|
||||
// significant sign value to orient N.
|
||||
float sign1 = b3Dot( n, b3Sub( p1, c1 ) );
|
||||
float sign2 = b3Dot( n, b3Sub( p2, c2 ) );
|
||||
if ( b3AbsFloat( sign1 ) > b3AbsFloat( sign2 ) )
|
||||
{
|
||||
if ( sign1 < 0.0f )
|
||||
{
|
||||
n = b3Neg( n );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( sign2 > 0.0f )
|
||||
{
|
||||
n = b3Neg( n );
|
||||
}
|
||||
}
|
||||
|
||||
// s = Dot(n, p2) - d = Dot(n, p2) - Dot(n, p1) = Dot(n, p2 - p1)
|
||||
return b3Dot( n, b3Sub( p2, p1 ) );
|
||||
}
|
||||
|
||||
// This was extended to make the wedge shape get the correct incident face.
|
||||
// Instead of looking directly for the most anti-parallel face, we first find the closest vertex (passed in).
|
||||
// Then we look for all edges coming out of that vertex and look for the edge that is
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@ typedef struct b3FaceQuery
|
|||
|
||||
typedef struct b3EdgeQuery
|
||||
{
|
||||
b3Vec3 normal;
|
||||
float separation;
|
||||
int indexA;
|
||||
int indexB;
|
||||
|
|
@ -35,7 +36,6 @@ typedef enum b3FeatureOwner
|
|||
b3_featureShapeB = 1
|
||||
} b3FeatureOwner;
|
||||
|
||||
float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 );
|
||||
int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex );
|
||||
b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 );
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@ typedef struct b3TriangleData
|
|||
{
|
||||
b3Vec3 v1, v2, v3;
|
||||
b3Vec3 e1, e2, e3;
|
||||
b3Vec3 center;
|
||||
b3Plane plane;
|
||||
int flags;
|
||||
} b3TriangleData;
|
||||
|
|
@ -178,35 +177,62 @@ static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule
|
|||
};
|
||||
}
|
||||
|
||||
static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const b3Capsule* capsule )
|
||||
static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )
|
||||
{
|
||||
// Work in the local space of the capsule
|
||||
b3Vec3 p1 = capsule->center1;
|
||||
b3Vec3 p2 = capsule->center2;
|
||||
b3Vec3 capsuleEdge = b3Sub( p2, p1 );
|
||||
|
||||
b3Vec3 capsuleCenter = b3Lerp( p1, p2, 0.5f );
|
||||
|
||||
b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vertices[0], b3Add( vertices[1], vertices[2] ) ) );
|
||||
|
||||
// Find axis of minimum penetration
|
||||
b3Vec3 maxNormal = b3Vec3_zero;
|
||||
float maxSeparation = -FLT_MAX;
|
||||
int maxIndex1 = UINT8_MAX;
|
||||
int maxIndex2 = UINT8_MAX;
|
||||
int maxIndex1 = B3_NULL_INDEX;
|
||||
int maxIndex2 = B3_NULL_INDEX;
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
|
||||
int edgeIndex = 2;
|
||||
b3Vec3 v1 = vertices[2];
|
||||
for ( int index = 0; index < 3; ++index )
|
||||
{
|
||||
b3Vec3 v2 = vertices[index];
|
||||
|
||||
b3Vec3 triangleEdge = b3Sub( v2, v1 );
|
||||
b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) );
|
||||
|
||||
// Pretend the triangle edge embeds a zero area face with a side normal. This
|
||||
// provides a way to find an edge-edge normal that points outward from
|
||||
// the triangle.
|
||||
float a = b3Dot( capsuleEdge, plane.normal );
|
||||
float b = b3Dot( capsuleEdge, sideNormal );
|
||||
|
||||
// Is the capsule edge parallel to the triangle edge? If so, face contact can handle it.
|
||||
if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Similar to hull vs hull (b3QueryEdgeDirections)
|
||||
b3Vec3 axis;
|
||||
if ( a * b <= 0.0f )
|
||||
{
|
||||
float t = b / ( b - a );
|
||||
axis = b3Lerp( sideNormal, plane.normal, t );
|
||||
}
|
||||
else
|
||||
{
|
||||
float t = b / ( a + b );
|
||||
axis = b3Lerp( sideNormal, b3Neg( plane.normal ), t );
|
||||
}
|
||||
|
||||
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
||||
axis = b3Normalize( axis );
|
||||
float separation = b3Dot( axis, b3Sub( p1, v1 ) );
|
||||
|
||||
float separation = b3EdgeEdgeSeparation( p1, capsuleEdge, capsuleCenter, v1, triangleEdge, triangleCenter );
|
||||
if ( separation > maxSeparation )
|
||||
{
|
||||
// Note: We don't exit early if we find a separating axis here since we want to
|
||||
// find the best one for caching and account for the convex radius later.
|
||||
maxNormal = axis;
|
||||
maxSeparation = separation;
|
||||
maxIndex1 = edgeIndex;
|
||||
maxIndex2 = 0;
|
||||
|
|
@ -218,9 +244,10 @@ static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const
|
|||
|
||||
// Save result
|
||||
return (b3EdgeQuery){
|
||||
.normal = maxNormal,
|
||||
.separation = maxSeparation,
|
||||
.indexA = (uint8_t)maxIndex1,
|
||||
.indexB = (uint8_t)maxIndex2,
|
||||
.indexA = maxIndex1,
|
||||
.indexB = maxIndex2,
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -272,8 +299,8 @@ static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, con
|
|||
pt->pair = segment[1].pair;
|
||||
}
|
||||
|
||||
static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, const b3Capsule* capsule,
|
||||
b3EdgeQuery query )
|
||||
static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
||||
const b3Capsule* capsule, b3EdgeQuery query )
|
||||
{
|
||||
B3_ASSERT( 0 <= query.indexA && query.indexA < 3 );
|
||||
|
||||
|
|
@ -283,20 +310,27 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con
|
|||
|
||||
const b3Vec3* vs = triangle;
|
||||
|
||||
b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vs[0], b3Add( vs[1], vs[2] ) ) );
|
||||
b3Vec3 v1 = vs[query.indexA];
|
||||
b3Vec3 v2 = vs[( query.indexA + 1 ) % 3];
|
||||
b3Vec3 triangleEdge = b3Sub( v2, v1 );
|
||||
|
||||
b3Vec3 normal = b3Cross( capsuleEdge, triangleEdge );
|
||||
normal = b3Normalize( normal );
|
||||
b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) );
|
||||
|
||||
// Normal should point away from triangle center
|
||||
if ( b3Dot( normal, b3Sub( v1, triangleCenter ) ) < 0.0f )
|
||||
// Pretend the triangle edge embeds a zero area face with a side normal. This
|
||||
// provides a way to find an edge-edge normal that points outward from
|
||||
// the triangle.
|
||||
float a = b3Dot( capsuleEdge, plane.normal );
|
||||
float b = b3Dot( capsuleEdge, sideNormal );
|
||||
|
||||
// Is the capsule edge parallel to the triangle edge? If so, face contact can handle it.
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) )
|
||||
{
|
||||
normal = b3Neg( normal );
|
||||
return;
|
||||
}
|
||||
|
||||
// Similar to hull vs hull (b3QueryEdgeDirections)
|
||||
b3Vec3 normal = query.normal;
|
||||
b3SegmentDistanceResult result = b3LineDistance( v1, triangleEdge, p1, capsuleEdge );
|
||||
|
||||
if ( result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 )
|
||||
|
|
@ -306,8 +340,7 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con
|
|||
}
|
||||
|
||||
b3Vec3 point = b3Lerp( b3MulSub( result.point1, capsule->radius, normal ), result.point2, 0.5f );
|
||||
|
||||
float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) );
|
||||
float separation = b3Dot( normal, b3Sub( p1, v1 ) );
|
||||
B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP );
|
||||
|
||||
manifold->normal = normal;
|
||||
|
|
@ -430,7 +463,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
return;
|
||||
}
|
||||
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, capsuleA );
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, plane, capsuleA );
|
||||
if ( edgeQuery.separation > radius )
|
||||
{
|
||||
// Shapes are separated
|
||||
|
|
@ -454,7 +487,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
|
|||
if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance )
|
||||
{
|
||||
// Edge contact
|
||||
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, capsuleA, edgeQuery );
|
||||
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, plane, capsuleA, edgeQuery );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -527,9 +560,10 @@ static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3Hull
|
|||
};
|
||||
}
|
||||
|
||||
static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )
|
||||
{
|
||||
b3EdgeQuery result = {
|
||||
.normal = b3Vec3_zero,
|
||||
.separation = -FLT_MAX,
|
||||
.indexA = B3_NULL_INDEX,
|
||||
.indexB = B3_NULL_INDEX,
|
||||
|
|
@ -552,6 +586,7 @@ static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3Hull
|
|||
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
||||
const b3Plane* hullPlanes = b3GetHullPlanes( hull );
|
||||
int edgeCount = hull->edgeCount;
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
|
||||
for ( int i = 0; i < edgeCount; i += 2 )
|
||||
{
|
||||
|
|
@ -577,14 +612,28 @@ static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3Hull
|
|||
continue;
|
||||
}
|
||||
|
||||
b3Vec3 triPoint = trianglePoints[j];
|
||||
float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangle->center, hullPoint, hullEdge, hull->center );
|
||||
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
||||
if ( b3MaxFloat( cab * cab, dab * dab ) < squaredTolerance * b3LengthSquared( triEdge ) )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Similar to hull vs hull (b3QueryEdgeDirections)
|
||||
// dot(hullNormal1 + t * (hullNormal2 - hullNormal1), triEdge) = 0
|
||||
// Normal points out of hull by construction.
|
||||
float t = cab / ( cab - dab );
|
||||
b3Vec3 axis = b3Lerp( hullNormal1, hullNormal2, t );
|
||||
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
||||
axis = b3Normalize( axis );
|
||||
float separation = b3Dot( axis, b3Sub( trianglePoints[j], hullPoint ) );
|
||||
|
||||
// if ( separation > result.separation && ( edgeFlags[j] & triangleFlags ) == 0 )
|
||||
if ( separation > result.separation )
|
||||
{
|
||||
// Note: We don't exit early if we find a separating axis here since we want to
|
||||
// find the best one for caching.
|
||||
// Flip normal to point from triangle to hull.
|
||||
result.normal = b3Neg( axis );
|
||||
result.separation = separation;
|
||||
result.indexA = j;
|
||||
result.indexB = i;
|
||||
|
|
@ -822,12 +871,11 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
|
|||
}
|
||||
|
||||
static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,
|
||||
b3Vec3 triangleCenter, const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache )
|
||||
const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache )
|
||||
{
|
||||
B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE );
|
||||
B3_ASSERT( query.indexA < 3 );
|
||||
|
||||
b3Vec3 cA = triangleCenter;
|
||||
b3Vec3 pA = trianglePoint;
|
||||
b3Vec3 eA = triangleEdge;
|
||||
|
||||
|
|
@ -839,32 +887,6 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci
|
|||
b3Vec3 qB = pointsB[twinB->origin];
|
||||
b3Vec3 eB = b3Sub( qB, pB );
|
||||
|
||||
b3Vec3 normal = b3Cross( eA, eB );
|
||||
normal = b3Normalize( normal );
|
||||
|
||||
// Ensure normal points outward from triangle center
|
||||
float outwardA = b3Dot( normal, b3Sub( pA, cA ) );
|
||||
|
||||
// Ensure normal points towards hull center
|
||||
float outwardB = b3Dot( normal, b3Sub( hull->center, pB ) );
|
||||
|
||||
// Use the largest magnitude. The triangle outward value
|
||||
// may be unreliable as some angles.
|
||||
if ( b3AbsFloat( outwardA ) > b3AbsFloat( outwardB ) )
|
||||
{
|
||||
if ( outwardA < 0.0f )
|
||||
{
|
||||
normal = b3Neg( normal );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( outwardB < 0.0f )
|
||||
{
|
||||
normal = b3Neg( normal );
|
||||
}
|
||||
}
|
||||
|
||||
// Get the closest points between the infinite edge lines
|
||||
b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB );
|
||||
|
||||
|
|
@ -879,7 +901,7 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci
|
|||
}
|
||||
|
||||
// This can slide off the end from caching
|
||||
float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) );
|
||||
float separation = b3Dot( query.normal, b3Sub( pB, pA ) );
|
||||
B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP );
|
||||
|
||||
b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) );
|
||||
|
|
@ -895,24 +917,13 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci
|
|||
cache->indexA = (uint8_t)query.indexA;
|
||||
cache->indexB = (uint8_t)query.indexB;
|
||||
|
||||
manifold->normal = normal;
|
||||
manifold->normal = query.normal;
|
||||
manifold->pointCount = 1;
|
||||
|
||||
b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 };
|
||||
manifold->feature = edgesFeatures[query.indexA];
|
||||
}
|
||||
|
||||
// See "Collision Detection of Convex Polyhedra Based on Duality Transformation"
|
||||
// Simplified for triangle versus hull
|
||||
static inline bool b3IsTriangleMinkowskiFace( b3Vec3 triNormal, b3Vec3 triEdge, b3Vec3 hullNormal1, b3Vec3 hullNormal2,
|
||||
b3Vec3 hullEdge )
|
||||
{
|
||||
float cab = b3Dot( hullNormal1, triEdge );
|
||||
float dab = b3Dot( hullNormal2, triEdge );
|
||||
float bcd = b3Dot( triNormal, hullEdge );
|
||||
return cab * dab < 0.0f && cab * bcd > 0.0f;
|
||||
}
|
||||
|
||||
b3AtomicInt b3_triangleConvexCalls;
|
||||
b3AtomicInt b3_triangleCacheHits;
|
||||
|
||||
|
|
@ -951,7 +962,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
return;
|
||||
}
|
||||
|
||||
b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) );
|
||||
b3Vec3 trianglePoints[] = { v1, v2, v3 };
|
||||
b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) };
|
||||
|
||||
|
|
@ -962,7 +972,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
.e1 = triangleEdges[0],
|
||||
.e2 = triangleEdges[1],
|
||||
.e3 = triangleEdges[2],
|
||||
.center = triangleCenter,
|
||||
.plane = trianglePlane,
|
||||
.flags = triangleFlags,
|
||||
};
|
||||
|
|
@ -1093,36 +1102,54 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
b3Vec3 hullNormal1 = hullPlanes[edge2->face].normal;
|
||||
b3Vec3 hullNormal2 = hullPlanes[twin2->face].normal;
|
||||
|
||||
// Confirm the edge pair is still a Minkowski face
|
||||
bool isMinkowski = b3IsTriangleMinkowskiFace( trianglePlane.normal, triEdge, hullNormal1, hullNormal2, hullEdge );
|
||||
if ( isMinkowski )
|
||||
// Confirm the edge pair is still a Minkowski face.
|
||||
// See "Collision Detection of Convex Polyhedra Based on Duality Transformation"
|
||||
// Simplified for triangle versus hull.
|
||||
float cab = b3Dot( hullNormal1, triEdge );
|
||||
float dab = b3Dot( hullNormal2, triEdge );
|
||||
float bcd = b3Dot( trianglePlane.normal, hullEdge );
|
||||
|
||||
if ( cab * dab < 0.0f && cab * bcd > 0.0f )
|
||||
{
|
||||
// Transform reference center of the first hull into local space of the second hull
|
||||
float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangleCenter, hullPoint, hullEdge, hullA->center );
|
||||
if ( separation > speculativeDistance )
|
||||
float squaredTolerance = 0.005f * 0.005f;
|
||||
|
||||
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
||||
if ( b3MaxFloat( cab * cab, dab * dab ) >= squaredTolerance * b3LengthSquared( triEdge ) )
|
||||
{
|
||||
// Cache hit, shapes are separated
|
||||
return;
|
||||
}
|
||||
|
||||
if ( b3AbsFloat( cache->separation - separation ) < linearSlop )
|
||||
{
|
||||
// Try to rebuild contact from last features
|
||||
b3EdgeQuery edgeQuery;
|
||||
edgeQuery.indexA = indexA;
|
||||
edgeQuery.indexB = indexB;
|
||||
edgeQuery.separation = separation;
|
||||
|
||||
// Read cache but don't modify it
|
||||
b3SATCache localCache = *cache;
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, triangleCenter, hullA, edgeQuery,
|
||||
&localCache );
|
||||
|
||||
if ( manifold->pointCount > 0 )
|
||||
// Similar to hull vs hull (b3QueryEdgeDirections)
|
||||
// dot(hullNormal1 + t * (hullNormal2 - hullNormal1), triEdge) = 0
|
||||
// Normal points out of hull by construction.
|
||||
float t = cab / ( cab - dab );
|
||||
b3Vec3 axis = b3Lerp( hullNormal1, hullNormal2, t );
|
||||
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
||||
axis = b3Normalize( axis );
|
||||
float separation = b3Dot( axis, b3Sub( triPoint, hullPoint ) );
|
||||
if ( separation > speculativeDistance )
|
||||
{
|
||||
// Cache hit, contact point generated
|
||||
// Cache hit, shapes are separated
|
||||
return;
|
||||
}
|
||||
|
||||
if ( b3AbsFloat( cache->separation - separation ) < linearSlop )
|
||||
{
|
||||
// Try to rebuild contact from last features
|
||||
// Flip normal to point from triangle to hull
|
||||
b3EdgeQuery edgeQuery;
|
||||
edgeQuery.normal = b3Neg( axis );
|
||||
edgeQuery.indexA = indexA;
|
||||
edgeQuery.indexB = indexB;
|
||||
edgeQuery.separation = separation;
|
||||
|
||||
// Read cache but don't modify it
|
||||
b3SATCache localCache = *cache;
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, hullA, edgeQuery, &localCache );
|
||||
|
||||
if ( manifold->pointCount > 0 )
|
||||
{
|
||||
// Cache hit, contact point generated
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1150,13 +1177,12 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
// This case is for testing
|
||||
case b3_manualEdgePairAxis:
|
||||
{
|
||||
b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA );
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA );
|
||||
if ( edgeQuery.indexA != B3_NULL_INDEX )
|
||||
{
|
||||
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
|
||||
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery,
|
||||
cache );
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache );
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
|
@ -1192,7 +1218,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
return;
|
||||
}
|
||||
|
||||
b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA );
|
||||
b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA );
|
||||
if ( edgeQuery.separation > speculativeDistance )
|
||||
{
|
||||
// Separating axis found
|
||||
|
|
@ -1234,8 +1260,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
|
|||
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
|
||||
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
|
||||
manifold->pointCount = 0;
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery,
|
||||
cache );
|
||||
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@ set(BOX3D_TEST_FILES
|
|||
test_joint.c
|
||||
test_large_world.c
|
||||
test_macros.h
|
||||
test_manifold.c
|
||||
test_math.c
|
||||
test_mover.c
|
||||
test_name_cache.c
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@ extern int HullTest( void );
|
|||
extern int IdTest( void );
|
||||
extern int JointTest( void );
|
||||
extern int LargeWorldTest( void );
|
||||
extern int ManifoldTest( void );
|
||||
extern int MathTest( void );
|
||||
extern int MoverTest( void );
|
||||
extern int NameCacheTest( void );
|
||||
|
|
@ -110,6 +111,7 @@ int main( int argc, char** argv )
|
|||
MAYBE_RUN_TEST( IdTest );
|
||||
MAYBE_RUN_TEST( JointTest );
|
||||
MAYBE_RUN_TEST( LargeWorldTest );
|
||||
MAYBE_RUN_TEST( ManifoldTest );
|
||||
MAYBE_RUN_TEST( MathTest );
|
||||
MAYBE_RUN_TEST( MoverTest );
|
||||
MAYBE_RUN_TEST( NameCacheTest );
|
||||
|
|
|
|||
1102
test/test_manifold.c
Normal file
1102
test/test_manifold.c
Normal file
File diff suppressed because it is too large
Load diff
Loading…
Reference in a new issue