Reduce ghosts (#212)
Full ghost collision handling for sphere, capsule, and hull versus a single mesh or heightfield. This requires mesh to be built with edge identification. Optimized mesh builder. Removed B3_MAX_MESH_CONTACT_TRIANGLES. You will still hit performance problems for dense meshes. Fixes #184
This commit is contained in:
parent
3362633204
commit
38f2ae1e19
61 changed files with 17756 additions and 1492 deletions
|
|
@ -83,6 +83,10 @@
|
|||
</Expand>
|
||||
</Type>
|
||||
|
||||
<Type Name="b3LocalManifoldPoint">
|
||||
<DisplayString>s: {separation}</DisplayString>
|
||||
</Type>
|
||||
|
||||
<Type Name="b3SeparationFunction">
|
||||
<DisplayString>type: {type}</DisplayString>
|
||||
<Expand>
|
||||
|
|
|
|||
|
|
@ -581,6 +581,8 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact*
|
|||
target->separation = source->separation;
|
||||
target->featureId = b3MakeFeatureId( source->pair );
|
||||
target->triangleIndex = B3_NULL_INDEX;
|
||||
target->triangleFeature = b3_featureNone;
|
||||
target->meshFlags = 0;
|
||||
target->normalVelocity = 0.0f;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -9,8 +9,6 @@
|
|||
#include "box3d/collision.h"
|
||||
#include "box3d/types.h"
|
||||
|
||||
#define B3_FORCE_GHOST_COLLISIONS 0
|
||||
|
||||
typedef struct b3Shape b3Shape;
|
||||
typedef struct b3World b3World;
|
||||
|
||||
|
|
|
|||
|
|
@ -990,7 +990,7 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L
|
|||
}
|
||||
|
||||
b3Point2D hull[2 * B3_MAX_CLIP_POINTS];
|
||||
int hullCount = b3Hull2D( pts, count, hull );
|
||||
int hullCount = b3Hull2D( pts, count, hull, 0 );
|
||||
int finalCount = b3SimplifyHull2D( hull, hullCount, target );
|
||||
B3_ASSERT( 0 < finalCount && finalCount <= target );
|
||||
|
||||
|
|
|
|||
|
|
@ -99,6 +99,53 @@
|
|||
21 3 22
|
||||
*/
|
||||
|
||||
static b3Vec3 b3GetGridPoint( const float* heights, int columnCount, b3Vec3 scale, int row, int column )
|
||||
{
|
||||
return b3Mul( scale, (b3Vec3){ (float)column, heights[row * columnCount + column], (float)row } );
|
||||
}
|
||||
|
||||
static b3Vec3 b3GetGridTriangleNormal( const float* heights, int columnCount, b3Vec3 scale, int row, int column, bool odd )
|
||||
{
|
||||
b3Vec3 point11 = b3GetGridPoint( heights, columnCount, scale, row, column );
|
||||
b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 );
|
||||
b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column );
|
||||
b3Vec3 point22 = b3GetGridPoint( heights, columnCount, scale, row + 1, column + 1 );
|
||||
|
||||
b3Vec3 v1 = odd ? point22 : point11;
|
||||
b3Vec3 v2 = odd ? point12 : point21;
|
||||
b3Vec3 v3 = odd ? point21 : point12;
|
||||
return b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) );
|
||||
}
|
||||
|
||||
// Mark vertices as flat or concave.
|
||||
static void b3MarkGridEdge( uint8_t* vertexFlags, int index1, int index2, b3Vec3 point1, b3Vec3 point2, b3Vec3 normal1,
|
||||
b3Vec3 normal2, bool clockwise )
|
||||
{
|
||||
b3Vec3 outward = b3Cross( b3Normalize( b3Sub( point2, point1 ) ), normal1 );
|
||||
float angle = b3Atan2( b3Dot( normal2, outward ), b3Dot( normal2, normal1 ) );
|
||||
|
||||
// Quantize to match the mesh rule.
|
||||
int8_t quantized = b3QuantizeEdgeAngle( clockwise ? -angle : angle );
|
||||
|
||||
if ( quantized != 0 )
|
||||
{
|
||||
vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex );
|
||||
vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex );
|
||||
}
|
||||
|
||||
if ( quantized < 0 )
|
||||
{
|
||||
vertexFlags[index1] |= b3_concaveVertex;
|
||||
vertexFlags[index2] |= b3_concaveVertex;
|
||||
}
|
||||
}
|
||||
|
||||
static void b3MarkGridBoundary( uint8_t* vertexFlags, int index1, int index2 )
|
||||
{
|
||||
vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex );
|
||||
vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex );
|
||||
}
|
||||
|
||||
b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
|
||||
{
|
||||
int columnCount = data->countX;
|
||||
|
|
@ -119,6 +166,8 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
|
|||
byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) );
|
||||
int flagsOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );
|
||||
int vertexFlagOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( (size_t)( ( heightCount + 7 ) / 8 ) );
|
||||
|
||||
// Zero the whole blob so alignment padding is defined. The construction-time hash
|
||||
// sweeps raw bytes and would otherwise pick up uninitialized padding.
|
||||
|
|
@ -133,6 +182,7 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
|
|||
hf->heightsOffset = heightsOffset;
|
||||
hf->materialOffset = materialOffset;
|
||||
hf->flagsOffset = flagsOffset;
|
||||
hf->vertexFlagOffset = vertexFlagOffset;
|
||||
hf->clockwise = data->clockwiseWinding;
|
||||
|
||||
uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset );
|
||||
|
|
@ -466,6 +516,79 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
|
|||
|
||||
B3_ASSERT( triangleIndex == triangleCount );
|
||||
|
||||
// b3MeshVertexFlags
|
||||
uint8_t* vertexFlags = (uint8_t*)b3Alloc( heightCount * sizeof( uint8_t ) );
|
||||
memset( vertexFlags, b3_flatVertex, heightCount * sizeof( uint8_t ) );
|
||||
|
||||
bool clockwise = hf->clockwise != 0;
|
||||
int cellColumnCount = columnCount - 1;
|
||||
int cellRowCount = rowCount - 1;
|
||||
|
||||
for ( int row = 0; row < rowCount; ++row )
|
||||
{
|
||||
for ( int column = 0; column < columnCount; ++column )
|
||||
{
|
||||
int index = row * columnCount + column;
|
||||
b3Vec3 point = b3GetGridPoint( heights, columnCount, scale, row, column );
|
||||
|
||||
if ( column < cellColumnCount )
|
||||
{
|
||||
bool below = row < cellRowCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE;
|
||||
bool above =
|
||||
row > 0 && materialIndices[( row - 1 ) * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE;
|
||||
if ( below && above )
|
||||
{
|
||||
b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row, column + 1 );
|
||||
b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false );
|
||||
b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row - 1, column, true );
|
||||
b3MarkGridEdge( vertexFlags, index + 1, index, next, point, normal1, normal2, clockwise );
|
||||
}
|
||||
else if ( below || above )
|
||||
{
|
||||
b3MarkGridBoundary( vertexFlags, index, index + 1 );
|
||||
}
|
||||
}
|
||||
|
||||
if ( row < cellRowCount )
|
||||
{
|
||||
bool right =
|
||||
column < cellColumnCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE;
|
||||
bool left = column > 0 && materialIndices[row * cellColumnCount + column - 1] != B3_HEIGHT_FIELD_HOLE;
|
||||
if ( right && left )
|
||||
{
|
||||
b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row + 1, column );
|
||||
b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false );
|
||||
b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column - 1, true );
|
||||
b3MarkGridEdge( vertexFlags, index, index + columnCount, point, next, normal1, normal2, clockwise );
|
||||
}
|
||||
else if ( right || left )
|
||||
{
|
||||
b3MarkGridBoundary( vertexFlags, index, index + columnCount );
|
||||
}
|
||||
}
|
||||
|
||||
if ( row < cellRowCount && column < cellColumnCount &&
|
||||
materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE )
|
||||
{
|
||||
b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column );
|
||||
b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 );
|
||||
b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false );
|
||||
b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, true );
|
||||
b3MarkGridEdge( vertexFlags, index + columnCount, index + 1, point21, point12, normal1, normal2, clockwise );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t* vertexBits = (uint8_t*)( (intptr_t)hf + vertexFlagOffset );
|
||||
for ( int i = 0; i < heightCount; ++i )
|
||||
{
|
||||
if ( vertexFlags[i] == 0 )
|
||||
{
|
||||
vertexBits[i >> 3] |= (uint8_t)( 1 << ( i & 7 ) );
|
||||
}
|
||||
}
|
||||
|
||||
b3Free( vertexFlags, heightCount * sizeof( uint8_t ) );
|
||||
b3Free( decompressedHeights, heightCount * sizeof( float ) );
|
||||
|
||||
hf->hash = 0;
|
||||
|
|
@ -474,9 +597,6 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
|
|||
return hf;
|
||||
}
|
||||
|
||||
_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" );
|
||||
_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" );
|
||||
|
||||
// Decode the four corner vertices of a height field cell into local space.
|
||||
// Output order matches the index naming used throughout this file:
|
||||
// corners[0] = (column, row), corners[1] = (column + 1, row),
|
||||
|
|
@ -518,8 +638,6 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t
|
|||
B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) );
|
||||
|
||||
b3Triangle triangle;
|
||||
triangle.flags = b3GetHeightFieldFlags( heightField )[triangleIndex];
|
||||
|
||||
int columnCount = heightField->columnCount;
|
||||
int quadIndex = triangleIndex >> 1;
|
||||
int row = quadIndex / ( columnCount - 1 );
|
||||
|
|
@ -562,15 +680,6 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t
|
|||
{
|
||||
B3_SWAP( triangle.vertices[1], triangle.vertices[2] );
|
||||
B3_SWAP( triangle.i2, triangle.i3 );
|
||||
|
||||
// Reversing winding swaps edge1 and edge3; edge2 (the diagonal) is preserved.
|
||||
int flags = triangle.flags;
|
||||
int edge1Bits = flags & ( b3_concaveEdge1 | b3_inverseConcaveEdge1 );
|
||||
int edge3Bits = flags & ( b3_concaveEdge3 | b3_inverseConcaveEdge3 );
|
||||
flags &= ~( b3_concaveEdge1 | b3_concaveEdge3 | b3_inverseConcaveEdge1 | b3_inverseConcaveEdge3 );
|
||||
flags |= edge1Bits << 2;
|
||||
flags |= edge3Bits >> 2;
|
||||
triangle.flags = flags;
|
||||
}
|
||||
|
||||
return triangle;
|
||||
|
|
@ -585,6 +694,124 @@ int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangle
|
|||
return b3GetHeightFieldMaterialIndices( heightField )[cellIndex];
|
||||
}
|
||||
|
||||
bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal )
|
||||
{
|
||||
B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) );
|
||||
B3_ASSERT( 0 <= edgeIndex && edgeIndex < 3 );
|
||||
|
||||
if ( heightField->clockwise )
|
||||
{
|
||||
// Reflected
|
||||
edgeIndex = 2 - edgeIndex;
|
||||
}
|
||||
|
||||
int cellColumnCount = heightField->columnCount - 1;
|
||||
int cellRowCount = heightField->rowCount - 1;
|
||||
int cellIndex = triangleIndex >> 1;
|
||||
int row = cellIndex / cellColumnCount;
|
||||
int column = cellIndex - row * cellColumnCount;
|
||||
bool odd = ( triangleIndex & 1 ) == 1;
|
||||
|
||||
// Get the row and column for the neighbor cell.
|
||||
if ( edgeIndex == 0 )
|
||||
{
|
||||
column += odd ? 1 : -1;
|
||||
}
|
||||
else if ( edgeIndex == 2 )
|
||||
{
|
||||
row += odd ? 1 : -1;
|
||||
}
|
||||
|
||||
if ( row < 0 || row >= cellRowCount || column < 0 || column >= cellColumnCount )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
int neighborCell = row * cellColumnCount + column;
|
||||
|
||||
const uint8_t* materialIndices = b3GetHeightFieldMaterialIndices( heightField );
|
||||
if ( materialIndices[neighborCell] == B3_HEIGHT_FIELD_HOLE )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
b3Triangle neighbor = b3GetHeightFieldTriangle( heightField, 2 * neighborCell + ( odd ? 0 : 1 ) );
|
||||
b3Vec3 e1 = b3Sub( neighbor.vertices[1], neighbor.vertices[0] );
|
||||
b3Vec3 e2 = b3Sub( neighbor.vertices[2], neighbor.vertices[0] );
|
||||
*normal = b3Normalize( b3Cross( e1, e2 ) );
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex )
|
||||
{
|
||||
B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) );
|
||||
B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 );
|
||||
|
||||
if ( heightField->vertexFlagOffset == 0 )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if ( heightField->clockwise )
|
||||
{
|
||||
vertexIndex = ( 3 - vertexIndex ) % 3;
|
||||
}
|
||||
|
||||
int columnCount = heightField->columnCount;
|
||||
int cellIndex = triangleIndex >> 1;
|
||||
int row = cellIndex / ( columnCount - 1 );
|
||||
int column = cellIndex - row * ( columnCount - 1 );
|
||||
|
||||
int offsets[2][3] = {
|
||||
{ 0, columnCount, 1 },
|
||||
{ columnCount + 1, 1, columnCount },
|
||||
};
|
||||
|
||||
int index = row * columnCount + column + offsets[triangleIndex & 1][vertexIndex];
|
||||
const uint8_t* vertexBits = (const uint8_t*)( (intptr_t)heightField + heightField->vertexFlagOffset );
|
||||
return ( vertexBits[index >> 3] & ( 1 << ( index & 7 ) ) ) != 0;
|
||||
}
|
||||
|
||||
int b3GetHeightFieldVertexTriangles( const b3HeightFieldData* heightField, int vertex, int triangles[6] )
|
||||
{
|
||||
int columnCount = heightField->columnCount;
|
||||
int cellColumnCount = columnCount - 1;
|
||||
int cellRowCount = heightField->rowCount - 1;
|
||||
int row = vertex / columnCount;
|
||||
int column = vertex - row * columnCount;
|
||||
|
||||
int rowOffsets[4] = { 0, 0, -1, -1 };
|
||||
int columnOffsets[4] = { 0, -1, 0, -1 };
|
||||
int firstParity[4] = { 0, 0, 0, 1 };
|
||||
int lastParity[4] = { 0, 1, 1, 1 };
|
||||
|
||||
const uint8_t* materialIndices = b3GetHeightFieldMaterialIndices( heightField );
|
||||
int count = 0;
|
||||
for ( int i = 0; i < 4; ++i )
|
||||
{
|
||||
int cellRow = row + rowOffsets[i];
|
||||
int cellColumn = column + columnOffsets[i];
|
||||
if ( cellRow < 0 || cellRow >= cellRowCount || cellColumn < 0 || cellColumn >= cellColumnCount )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
int cellIndex = cellRow * cellColumnCount + cellColumn;
|
||||
if ( materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
for ( int parity = firstParity[i]; parity <= lastParity[i]; ++parity )
|
||||
{
|
||||
triangles[count] = 2 * cellIndex + parity;
|
||||
count += 1;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform )
|
||||
{
|
||||
return b3AABB_Transform( transform, shape->aabb );
|
||||
|
|
|
|||
215
src/hull.c
215
src/hull.c
|
|
@ -7,6 +7,7 @@
|
|||
|
||||
#include "algorithm.h"
|
||||
#include "math_internal.h"
|
||||
#include "qsort.h"
|
||||
#include "shape.h"
|
||||
#include "simd.h"
|
||||
|
||||
|
|
@ -398,7 +399,8 @@ static void b3FindFarthestPointsAlongCardinalAxes( int* index1Out, int* index2Ou
|
|||
}
|
||||
}
|
||||
|
||||
static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, const b3Vec3* vertexBase )
|
||||
static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount,
|
||||
const b3Vec3* vertexBase )
|
||||
{
|
||||
b3Vec3 a = vertexBase[index1];
|
||||
b3Vec3 b = vertexBase[index2];
|
||||
|
|
@ -764,7 +766,8 @@ static void b3HullBuilder_RecacheConflicts( b3QHFace* face, float minOutside )
|
|||
b3QHVertex* maxVertex = NULL;
|
||||
float maxDistance = minOutside;
|
||||
|
||||
for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; node = node->next )
|
||||
for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link;
|
||||
node = node->next )
|
||||
{
|
||||
b3QHVertex* vertex = (b3QHVertex*)node;
|
||||
float distance = b3PlaneSeparation( face->plane, vertex->position );
|
||||
|
|
@ -784,7 +787,8 @@ static b3QHVertex* b3HullBuilder_NextConflictVertex( const b3HullBuilder* b )
|
|||
b3QHVertex* maxVertex = NULL;
|
||||
float maxDistance = b->minOutside;
|
||||
|
||||
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next )
|
||||
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link;
|
||||
faceNode = faceNode->next )
|
||||
{
|
||||
const b3QHFace* face = (const b3QHFace*)faceNode;
|
||||
if ( face->maxConflict != NULL && face->maxConflictDistance > maxDistance )
|
||||
|
|
@ -817,7 +821,8 @@ static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face )
|
|||
// `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or
|
||||
// NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored
|
||||
// anyway since the parent's mark is now DELETE, but skipping saves one iteration).
|
||||
static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, b3HorizonFrame* frameOut )
|
||||
static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge,
|
||||
b3HorizonFrame* frameOut )
|
||||
{
|
||||
face->mark = B3_MARK_DELETE;
|
||||
b3HullBuilder_DrainConflictList( b, face );
|
||||
|
|
@ -1357,7 +1362,8 @@ static bool b3HullBuilder_IsConsistent( const b3HullBuilder* b )
|
|||
return false;
|
||||
}
|
||||
|
||||
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next )
|
||||
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link;
|
||||
faceNode = faceNode->next )
|
||||
{
|
||||
const b3QHFace* face = (const b3QHFace*)faceNode;
|
||||
if ( face->edge->face != face )
|
||||
|
|
@ -1427,8 +1433,8 @@ static bool b3HullBuilder_HasHull( const b3HullBuilder* b )
|
|||
}
|
||||
|
||||
// Build the entire hull. Returns true iff the result satisfies Euler's identity.
|
||||
static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, b3Vec3 origin,
|
||||
b3Vec3* shiftedPoints )
|
||||
static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount,
|
||||
b3Vec3 origin, b3Vec3* shiftedPoints )
|
||||
{
|
||||
if ( pointCount < 4 )
|
||||
{
|
||||
|
|
@ -2195,7 +2201,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
int faceCount = 0;
|
||||
int edgeCount = 0;
|
||||
|
||||
for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; faceNode = faceNode->next )
|
||||
for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link;
|
||||
faceNode = faceNode->next )
|
||||
{
|
||||
B3_ASSERT( faceCount < B3_HULL_MAX_COUNT );
|
||||
|
||||
|
|
@ -3098,20 +3105,20 @@ int b3SimplifyHull2D( b3Point2D* hull, int count1, int target )
|
|||
return count2;
|
||||
}
|
||||
|
||||
// Sort comparator: by x, then by y. Insertion since n is typically 20 or less.
|
||||
static void b3Sort2D( b3Point2D* pts, int count )
|
||||
// Comparator for sorting points for the 2D hull.
|
||||
static inline bool b3IsPoint2DLess( const b3Point2D* a, const b3Point2D* b )
|
||||
{
|
||||
for ( int i = 1; i < count; ++i )
|
||||
if ( a->p.x != b->p.x )
|
||||
{
|
||||
b3Point2D base = pts[i];
|
||||
int j = i - 1;
|
||||
while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) )
|
||||
{
|
||||
pts[j + 1] = pts[j];
|
||||
j -= 1;
|
||||
}
|
||||
pts[j + 1] = base;
|
||||
return a->p.x < b->p.x;
|
||||
}
|
||||
|
||||
if ( a->p.y != b->p.y )
|
||||
{
|
||||
return a->p.y < b->p.y;
|
||||
}
|
||||
|
||||
return a->originalIndex < b->originalIndex;
|
||||
}
|
||||
|
||||
// Welds points that have been sorted O(n) time
|
||||
|
|
@ -3148,79 +3155,151 @@ static int b3Weld2D( b3Point2D* pts, int count1 )
|
|||
return count2;
|
||||
}
|
||||
|
||||
// Only merge points that line up exactly.
|
||||
static int b3MergeExact2D( b3Point2D* pts, int count1 )
|
||||
{
|
||||
float separationTol = B3_LINEAR_SLOP;
|
||||
|
||||
int count2 = 0;
|
||||
for ( int i = 0; i < count1; i++ )
|
||||
{
|
||||
b3Point2D point = pts[i];
|
||||
if ( count2 > 0 && pts[count2 - 1].p.x == point.p.x && pts[count2 - 1].p.y == point.p.y )
|
||||
{
|
||||
if ( point.separation < pts[count2 - 1].separation - separationTol )
|
||||
{
|
||||
pts[count2 - 1] = point;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pts[count2] = point;
|
||||
count2 += 1;
|
||||
}
|
||||
}
|
||||
|
||||
return count2;
|
||||
}
|
||||
|
||||
// Andrew's monotone chain convex hull algorithm
|
||||
// hull must have space for 2 * count1 points
|
||||
int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull )
|
||||
int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull, int flags )
|
||||
{
|
||||
int count1 = count;
|
||||
|
||||
if ( count1 <= 0 )
|
||||
if ( count <= 0 )
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
if ( count1 == 1 )
|
||||
#define LESS( i, j ) b3IsPoint2DLess( pts + (int)( i ), pts + (int)( j ) )
|
||||
#define SWAP( i, j ) \
|
||||
do \
|
||||
{ \
|
||||
b3Point2D swapPoint = pts[(int)( i )]; \
|
||||
pts[(int)( i )] = pts[(int)( j )]; \
|
||||
pts[(int)( j )] = swapPoint; \
|
||||
} \
|
||||
while ( 0 )
|
||||
|
||||
QSORT( count, LESS, SWAP );
|
||||
|
||||
#undef LESS
|
||||
#undef SWAP
|
||||
|
||||
int count1;
|
||||
if ( flags & b3_hull2DExactMerge )
|
||||
{
|
||||
hull[0] = pts[0];
|
||||
return 1;
|
||||
count1 = b3MergeExact2D( pts, count );
|
||||
}
|
||||
else
|
||||
{
|
||||
count1 = b3Weld2D( pts, count );
|
||||
}
|
||||
|
||||
b3Sort2D( pts, count1 );
|
||||
count1 = b3Weld2D( pts, count1 );
|
||||
|
||||
if ( count1 == 1 )
|
||||
{
|
||||
hull[0] = pts[0];
|
||||
return 1;
|
||||
}
|
||||
|
||||
if ( count1 == 2 )
|
||||
{
|
||||
hull[0] = pts[0];
|
||||
hull[1] = pts[1];
|
||||
return 2;
|
||||
}
|
||||
bool keepCollinear = ( flags & b3_hull2DKeepCollinear ) != 0;
|
||||
|
||||
int count2 = 0;
|
||||
|
||||
for ( int i = 0; i < count1; i++ )
|
||||
if ( count1 <= 2 )
|
||||
{
|
||||
while ( count2 >= 2 )
|
||||
for ( int i = 0; i < count1; i++ )
|
||||
{
|
||||
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
|
||||
if ( area > 0.0f )
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
count2 -= 1;
|
||||
hull[i] = pts[i];
|
||||
}
|
||||
|
||||
B3_VALIDATE( count2 < 2 * count1 );
|
||||
hull[count2] = pts[i];
|
||||
count2 += 1;
|
||||
count2 = count1;
|
||||
}
|
||||
|
||||
int lowerCount = count2 + 1;
|
||||
|
||||
for ( int i = count1 - 2; i >= 0; i-- )
|
||||
else
|
||||
{
|
||||
while ( count2 >= lowerCount )
|
||||
for ( int i = 0; i < count1; i++ )
|
||||
{
|
||||
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
|
||||
if ( area > 0.0f )
|
||||
while ( count2 >= 2 )
|
||||
{
|
||||
break;
|
||||
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
|
||||
if ( area > 0.0f || ( keepCollinear && area == 0.0f ) )
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
count2 -= 1;
|
||||
}
|
||||
|
||||
count2 -= 1;
|
||||
B3_VALIDATE( count2 < 2 * count1 );
|
||||
hull[count2] = pts[i];
|
||||
count2 += 1;
|
||||
}
|
||||
|
||||
B3_VALIDATE( count2 < 2 * count1 );
|
||||
hull[count2] = pts[i];
|
||||
count2 += 1;
|
||||
int lowerCount = count2 + 1;
|
||||
int lowerIndex = count2 - 2;
|
||||
|
||||
for ( int i = count1 - 2; i >= 0; i-- )
|
||||
{
|
||||
if ( lowerIndex >= 0 && hull[lowerIndex].p.x == pts[i].p.x && hull[lowerIndex].p.y == pts[i].p.y )
|
||||
{
|
||||
lowerIndex -= 1;
|
||||
if ( i > 0 )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
while ( count2 >= lowerCount )
|
||||
{
|
||||
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
|
||||
if ( area > 0.0f || ( keepCollinear && area == 0.0f ) )
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
count2 -= 1;
|
||||
}
|
||||
|
||||
B3_VALIDATE( count2 < 2 * count1 );
|
||||
hull[count2] = pts[i];
|
||||
count2 += 1;
|
||||
}
|
||||
|
||||
B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex );
|
||||
count2 -= 1;
|
||||
}
|
||||
|
||||
B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex );
|
||||
return count2 - 1;
|
||||
B3_VALIDATE( count2 <= count1 );
|
||||
|
||||
if ( flags & b3_hull2DIndexOrder )
|
||||
{
|
||||
#define LESS( i, j ) ( hull[(int)( i )].originalIndex < hull[(int)( j )].originalIndex )
|
||||
#define SWAP( i, j ) \
|
||||
do \
|
||||
{ \
|
||||
b3Point2D swapPoint = hull[(int)( i )]; \
|
||||
hull[(int)( i )] = hull[(int)( j )]; \
|
||||
hull[(int)( j )] = swapPoint; \
|
||||
} \
|
||||
while ( 0 )
|
||||
|
||||
QSORT( count2, LESS, SWAP );
|
||||
|
||||
#undef LESS
|
||||
#undef SWAP
|
||||
}
|
||||
|
||||
return count2;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -51,6 +51,12 @@ b3AxisQuery b3ComputeSeparatingAxisAtWidth( const b3HullData* hullA, const b3Hul
|
|||
void b3CollideHullsAtWidth( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
|
||||
b3Transform transformBtoA, b3SATCache* cache, int simdWidth );
|
||||
|
||||
void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
||||
const b3Capsule* capsule );
|
||||
|
||||
void b3BuildTriangleAndHullFaceContact( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, const b3HullData* hull,
|
||||
b3SATCache* cache, bool enableSpeculative );
|
||||
|
||||
b3AxisQuery b3ComputeSeparatingAxisW4( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );
|
||||
b3AxisQuery b3ComputeSeparatingAxisW8( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );
|
||||
|
||||
|
|
|
|||
|
|
@ -166,49 +166,6 @@ bool b3IsValidWorldTransform( b3WorldTransform t )
|
|||
return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q );
|
||||
}
|
||||
|
||||
// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
|
||||
float b3Atan2( float y, float x )
|
||||
{
|
||||
// Added check for (0,0) to match atan2f and avoid NaN
|
||||
if ( x == 0.0f && y == 0.0f )
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float ax = b3AbsFloat( x );
|
||||
float ay = b3AbsFloat( y );
|
||||
float mx = b3MaxFloat( ay, ax );
|
||||
float mn = b3MinFloat( ay, ax );
|
||||
float a = mn / mx;
|
||||
|
||||
// Minimax polynomial approximation to atan(a) on [0,1]
|
||||
float s = a * a;
|
||||
float c = s * a;
|
||||
float q = s * s;
|
||||
float r = 0.024840285f * q + 0.18681418f;
|
||||
float t = -0.094097948f * q - 0.33213072f;
|
||||
r = r * s + t;
|
||||
r = r * c + a;
|
||||
|
||||
// Map to full circle
|
||||
if ( ay > ax )
|
||||
{
|
||||
r = 1.57079637f - r;
|
||||
}
|
||||
|
||||
if ( x < 0 )
|
||||
{
|
||||
r = 3.14159274f - r;
|
||||
}
|
||||
|
||||
if ( y < 0 )
|
||||
{
|
||||
r = -r;
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
// Approximate cosine and sine for determinism. In my testing cosf and sinf produced
|
||||
// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust
|
||||
// this result.
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@ typedef struct b3Triangle
|
|||
{
|
||||
b3Vec3 vertices[3];
|
||||
int i1, i2, i3;
|
||||
int flags;
|
||||
} b3Triangle;
|
||||
|
||||
typedef struct b3TrianglePoint
|
||||
|
|
|
|||
1244
src/mesh.c
1244
src/mesh.c
File diff suppressed because it is too large
Load diff
1671
src/mesh_contact.c
1671
src/mesh_contact.c
File diff suppressed because it is too large
Load diff
42
src/shape.h
42
src/shape.h
|
|
@ -129,6 +129,10 @@ float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction );
|
|||
b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex );
|
||||
int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex );
|
||||
|
||||
bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal );
|
||||
bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex );
|
||||
int b3GetHeightFieldVertexTriangles( const b3HeightFieldData* heightField, int vertex, int triangles[6] );
|
||||
|
||||
static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* heightField )
|
||||
{
|
||||
int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 );
|
||||
|
|
@ -143,12 +147,10 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde
|
|||
B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount );
|
||||
|
||||
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data );
|
||||
const uint8_t* flags = b3GetMeshFlags( mesh->data );
|
||||
const b3Vec3* vertices = b3GetMeshVertices( mesh->data );
|
||||
|
||||
b3Triangle result;
|
||||
b3MeshTriangle triangle = triangles[triangleIndex];
|
||||
uint8_t triangleFlags = flags[triangleIndex];
|
||||
|
||||
b3Vec3 scale = mesh->scale;
|
||||
|
||||
|
|
@ -163,11 +165,6 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde
|
|||
result.i2 = triangle.index3;
|
||||
result.i3 = triangle.index2;
|
||||
|
||||
// mesh is inverted, so concave edges are now convex
|
||||
result.flags = 0;
|
||||
result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0;
|
||||
result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0;
|
||||
result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -176,12 +173,41 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde
|
|||
|
||||
result.i2 = triangle.index2;
|
||||
result.i3 = triangle.index3;
|
||||
result.flags = triangleFlags;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool b3GetMeshNeighborNormal( const b3Mesh* mesh, int triangleIndex, int edgeIndex, b3Vec3* normal );
|
||||
|
||||
// Returns false if edge is isolated.
|
||||
static inline bool b3GetNeighborNormal( const b3Shape* shape, int triangleIndex, int edgeIndex, b3Vec3* normal )
|
||||
{
|
||||
if ( shape->type == b3_meshShape )
|
||||
{
|
||||
return b3GetMeshNeighborNormal( &shape->mesh, triangleIndex, edgeIndex, normal );
|
||||
}
|
||||
|
||||
B3_ASSERT( shape->type == b3_heightShape );
|
||||
return b3GetHeightFieldNeighborNormal( shape->heightField, triangleIndex, edgeIndex, normal );
|
||||
}
|
||||
|
||||
int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex );
|
||||
bool b3IsInMeshVertexRing( const b3Mesh* mesh, int triangleIndex, int vertexIndex, b3Vec3 normal, float tolerance );
|
||||
int8_t b3QuantizeEdgeAngle( float radians );
|
||||
|
||||
static inline bool b3IsConvexVertex( const b3Shape* shape, int triangleIndex, int vertexIndex )
|
||||
{
|
||||
if ( shape->type == b3_meshShape )
|
||||
{
|
||||
int flags = b3GetMeshVertexFlag( &shape->mesh, triangleIndex, vertexIndex );
|
||||
return ( flags & ( b3_flatVertex | b3_concaveVertex ) ) == 0;
|
||||
}
|
||||
|
||||
B3_ASSERT( shape->type == b3_heightShape );
|
||||
return b3IsHeightFieldVertexConvex( shape->heightField, triangleIndex, vertexIndex );
|
||||
}
|
||||
|
||||
bool b3IsValidMesh( const b3MeshData* meshData );
|
||||
|
||||
static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB )
|
||||
|
|
|
|||
24
src/simd.h
24
src/simd.h
|
|
@ -29,6 +29,30 @@
|
|||
|
||||
#endif
|
||||
|
||||
static inline b3FloatW4 b3Atan2W4( b3FloatW4 y, b3FloatW4 x )
|
||||
{
|
||||
b3FloatW4 zero = b3ZeroW4();
|
||||
b3FloatW4 ax = b3AbsW4( x );
|
||||
b3FloatW4 ay = b3AbsW4( y );
|
||||
b3FloatW4 mx = b3MaxW4( ay, ax );
|
||||
b3FloatW4 mn = b3MinW4( ay, ax );
|
||||
b3FloatW4 a = b3DivW4( mn, mx );
|
||||
|
||||
b3FloatW4 s = b3MulW4( a, a );
|
||||
b3FloatW4 c = b3MulW4( s, a );
|
||||
b3FloatW4 q = b3MulW4( s, s );
|
||||
b3FloatW4 r = b3MulAddW4( b3SplatW4( 0.18681418f ), b3SplatW4( 0.024840285f ), q );
|
||||
b3FloatW4 t = b3SubW4( b3MulW4( b3SplatW4( -0.094097948f ), q ), b3SplatW4( 0.33213072f ) );
|
||||
r = b3MulAddW4( t, r, s );
|
||||
r = b3MulAddW4( a, r, c );
|
||||
|
||||
r = b3BlendW4( r, b3SubW4( b3SplatW4( 1.57079637f ), r ), b3GreaterThanW4( ay, ax ) );
|
||||
r = b3BlendW4( r, b3SubW4( b3SplatW4( 3.14159274f ), r ), b3LessThanW4( x, zero ) );
|
||||
r = b3BlendW4( r, b3NegW4( r ), b3LessThanW4( y, zero ) );
|
||||
|
||||
return b3BlendW4( r, zero, b3EqualsW4( b3AddW4( ax, ay ), zero ) );
|
||||
}
|
||||
|
||||
#if !defined( B3_SIMD_SSE2 )
|
||||
|
||||
// I don't expect the use case of b3V32 to benefit from Neon code.
|
||||
|
|
|
|||
|
|
@ -14,12 +14,14 @@
|
|||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
// cosine of hull normal versus edge axis tolerance.
|
||||
#define B3_CROSSING_TOLERANCE 0.1f
|
||||
|
||||
typedef struct b3TriangleData
|
||||
{
|
||||
b3Vec3 v1, v2, v3;
|
||||
b3Vec3 e1, e2, e3;
|
||||
b3Plane plane;
|
||||
int flags;
|
||||
} b3TriangleData;
|
||||
|
||||
// Indexed by the 3-bit vertex mask
|
||||
|
|
@ -104,7 +106,6 @@ void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const
|
|||
manifold->normal = normal;
|
||||
manifold->pointCount = 1;
|
||||
manifold->feature = closest.feature;
|
||||
manifold->squaredDistance = squaredDistance;
|
||||
|
||||
b3LocalManifoldPoint* mp = manifold->points + 0;
|
||||
mp->point = contactPoint;
|
||||
|
|
@ -260,8 +261,8 @@ static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices,
|
|||
};
|
||||
}
|
||||
|
||||
static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
||||
const b3Capsule* capsule )
|
||||
void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
||||
const b3Capsule* capsule )
|
||||
{
|
||||
B3_ASSERT( manifold->pointCount == 0 );
|
||||
|
||||
|
|
@ -597,15 +598,6 @@ static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* trian
|
|||
|
||||
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
|
||||
b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 };
|
||||
// int edgeFlags[] = { b3_concaveEdge1, b3_concaveEdge1, b3_concaveEdge3 };
|
||||
|
||||
#if B3_FORCE_GHOST_COLLISIONS
|
||||
int triangleFlags = 0xFF;
|
||||
#else
|
||||
int triangleFlags = triangle->flags;
|
||||
#endif
|
||||
(void)triangleFlags;
|
||||
|
||||
b3Vec3 triNormal = triangle->plane.normal;
|
||||
|
||||
const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull );
|
||||
|
|
@ -653,7 +645,6 @@ static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* trian
|
|||
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
|
||||
|
|
@ -748,7 +739,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
|
|||
b3ClipVertex* clipPoint = input + i;
|
||||
minSeparation = b3MinFloat( minSeparation, clipPoint->separation );
|
||||
|
||||
if ( enableSpeculative == false && clipPoint->separation > 0.0f )
|
||||
if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
|
@ -764,7 +755,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
|
|||
finalPointCount += 1;
|
||||
}
|
||||
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET;
|
||||
if ( minSeparation > speculativeDistance )
|
||||
{
|
||||
// This can occur with a stale SAT cache
|
||||
|
|
@ -864,7 +855,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
|
|||
b3ClipVertex* clipPoint = input + i;
|
||||
minSeparation = b3MinFloat( minSeparation, clipPoint->separation );
|
||||
|
||||
if ( enableSpeculative == false && clipPoint->separation > 0.0f )
|
||||
if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
|
@ -881,7 +872,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
|
|||
finalPointCount += 1;
|
||||
}
|
||||
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET;
|
||||
if ( minSeparation >= speculativeDistance )
|
||||
{
|
||||
// This can happens if the objects move a part while re-using a cached axis
|
||||
|
|
@ -954,11 +945,37 @@ static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capaci
|
|||
manifold->feature = edgesFeatures[query.indexA];
|
||||
}
|
||||
|
||||
// Triangle face contact wrapper.
|
||||
void b3BuildTriangleAndHullFaceContact( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, const b3HullData* hull,
|
||||
b3SATCache* cache, bool enableSpeculative )
|
||||
{
|
||||
manifold->pointCount = 0;
|
||||
|
||||
b3TriangleData triangleData = {
|
||||
.v1 = triangle[0],
|
||||
.v2 = triangle[1],
|
||||
.v3 = triangle[2],
|
||||
.e1 = b3Sub( triangle[1], triangle[0] ),
|
||||
.e2 = b3Sub( triangle[2], triangle[1] ),
|
||||
.e3 = b3Sub( triangle[0], triangle[2] ),
|
||||
.plane = b3MakePlaneFromPoints( triangle[0], triangle[1], triangle[2] ),
|
||||
};
|
||||
|
||||
b3SeparatingAxis query = b3QueryTriangleFace( &triangleData, hull );
|
||||
b3SATCache faceCache = { 0 };
|
||||
b3CollideTriangleFace( manifold, capacity, &triangleData, hull, query, &faceCache, enableSpeculative );
|
||||
|
||||
if ( manifold->pointCount > 0 )
|
||||
{
|
||||
*cache = faceCache;
|
||||
}
|
||||
}
|
||||
|
||||
b3AtomicInt b3_triangleConvexCalls;
|
||||
b3AtomicInt b3_triangleCacheHits;
|
||||
|
||||
// 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,
|
||||
void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3,
|
||||
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative )
|
||||
{
|
||||
manifold->pointCount = 0;
|
||||
|
|
@ -1003,14 +1020,13 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
|
|||
.e2 = triangleEdges[1],
|
||||
.e3 = triangleEdges[2],
|
||||
.plane = trianglePlane,
|
||||
.flags = triangleFlags,
|
||||
};
|
||||
|
||||
const b3HullHalfEdge* edges = b3GetHullEdges( hullB );
|
||||
const b3Plane* hullPlanes = b3GetHullPlanes( hullB );
|
||||
const b3Vec3* hullPoints = b3GetHullPoints( hullB );
|
||||
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
||||
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET;
|
||||
cache->hit = 1;
|
||||
|
||||
// Attempt to use the cache to speed up collision
|
||||
|
|
@ -1082,6 +1098,9 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
|
|||
return;
|
||||
}
|
||||
|
||||
// Note: it is possible for a hull face to lean into an edge direction while mainting
|
||||
// a valid cache. But this has never been observed.
|
||||
|
||||
// Deep overlap may lead to an invalid cache
|
||||
// todo confirm
|
||||
bool isDeep = separation < -2.0f * linearSlop;
|
||||
|
|
@ -1285,14 +1304,29 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
|
|||
// If a face axis has positive separation there may be no points.
|
||||
float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation );
|
||||
|
||||
// The edge axis can win if the hull face axis is significantly aligned with the edge axis.
|
||||
bool crossesEdge = false;
|
||||
if ( manifold->pointCount >= 1 && manifold->feature == b3_featureHullFace && edgeQuery.separation > clipSeparation )
|
||||
{
|
||||
b3Vec3 edgeAxis = b3Normalize( triangleEdges[edgeQuery.indexA] );
|
||||
|
||||
// Is the angle less than ~84 degrees?
|
||||
crossesEdge = b3AbsFloat( b3Dot( faceQueryB.normal, edgeAxis ) ) > B3_CROSSING_TOLERANCE;
|
||||
}
|
||||
|
||||
if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) ||
|
||||
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) )
|
||||
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) || crossesEdge )
|
||||
{
|
||||
B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 );
|
||||
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
|
||||
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
|
||||
manifold->pointCount = 0;
|
||||
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache );
|
||||
|
||||
if ( crossesEdge && manifold->pointCount == 0 )
|
||||
{
|
||||
b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1314,7 +1348,7 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
|
|||
b3SimplexCache simplexCache = { 0 };
|
||||
b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 );
|
||||
|
||||
if ( output.distance > 0.0f )
|
||||
if ( 0.0f < output.distance && output.distance <= speculativeDistance )
|
||||
{
|
||||
B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 );
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue