clockwise option, joint is awake (#130)
- b3MeshDef gets stride and clockwise option - Added b3Joint_IsAwake - Added b3Body_TimeOfImpactMover - #116 - Smoother mover collision: #141 - Shape casts now ignore backside collision on mesh and height-field triangles, same as ray casts.
This commit is contained in:
parent
30c67b5e6d
commit
47d7f7cc7e
35 changed files with 2466 additions and 303 deletions
11
.github/issue_template.md
vendored
11
.github/issue_template.md
vendored
|
|
@ -1 +1,10 @@
|
|||
Thank you for filing an issue!
|
||||
|
||||
## Reporting bugs
|
||||
|
||||
Bugs can be filed by humans or LLMs. Please help me reproduce your bug. Include a sample, unit test, or recording if possible. Recordings
|
||||
should make note of the Box3D commit used (the SHA).
|
||||
|
||||
## Feature requests
|
||||
|
||||
Please don't disguise feature requests as bugs. Feature requests must be human typed because they usually involve discussion.
|
||||
I'm likely to close feature requests otherwise.
|
||||
|
|
|
|||
1
.gitignore
vendored
1
.gitignore
vendored
|
|
@ -14,3 +14,4 @@ CMakeUserPresets.json
|
|||
*.b3rec
|
||||
Superluminal/
|
||||
build-dp/
|
||||
AGENTS.md
|
||||
|
|
|
|||
|
|
@ -82,6 +82,7 @@ B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const
|
|||
|
||||
/// Cast a ray into the world to collect shapes in the path of the ray.
|
||||
/// Your callback function controls whether you get the closest point, any point, or n-points.
|
||||
/// Ignores back-side collision on meshes and height-fields.
|
||||
/// @note The callback function may receive shapes in any order
|
||||
/// @param worldId The world to cast the ray against
|
||||
/// @param origin The start point of the ray
|
||||
|
|
@ -94,19 +95,20 @@ B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 tran
|
|||
b3CastResultFcn* fcn, void* context );
|
||||
|
||||
/// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap.
|
||||
/// This is less general than b3World_CastRay() and does not allow for custom filtering.
|
||||
/// This is less general than b3World_CastRay() and does not allow for custom filtering. Ignores back-side collision
|
||||
/// on meshes and height-fields.
|
||||
B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter );
|
||||
|
||||
/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point.
|
||||
/// The proxy points are relative to the origin and the hit points come back as world positions, so the
|
||||
/// cast stays precise far from the world origin.
|
||||
/// cast stays precise far from the world origin. Ignores back-side collision on meshes and height-fields.
|
||||
/// @see b3World_CastRay
|
||||
B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
|
||||
b3QueryFilter filter, b3CastResultFcn* fcn, void* context );
|
||||
|
||||
/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing
|
||||
/// clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by
|
||||
/// b3World_CollideMover.
|
||||
/// b3World_CollideMover. Ignores back-side collision on meshes and height-fields.
|
||||
/// @param worldId World to cast the mover against
|
||||
/// @param origin World position the mover capsule is relative to
|
||||
/// @param mover Capsule mover, relative to the origin
|
||||
|
|
@ -120,6 +122,7 @@ B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule
|
|||
|
||||
/// Collide a capsule mover with the world, gathering collision planes that can be fed to b3SolvePlanes. Useful for
|
||||
/// kinematic character movement. The mover and the returned planes are relative to the origin.
|
||||
/// Ignores back-side collision on meshes and height-fields.
|
||||
B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter,
|
||||
b3PlaneResultFcn* fcn, void* context );
|
||||
|
||||
|
|
@ -759,14 +762,25 @@ B3_API int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, i
|
|||
/// If there are no shapes attached then the returned AABB is empty and centered on the body origin.
|
||||
B3_API b3AABB b3Body_ComputeAABB( b3BodyId bodyId );
|
||||
|
||||
/// The minimum distance from any shape to the shape centroid.
|
||||
B3_API float b3Body_GetMinExtent( b3BodyId bodyId );
|
||||
|
||||
/// The maximum extent vector from any point on the body shapes to the center of mass.
|
||||
B3_API b3Vec3 b3Body_GetMaxExtent( b3BodyId bodyId );
|
||||
|
||||
/// The maximum extent vector from any point on the body shapes to the body origin. Conservative.
|
||||
B3_API b3Vec3 b3Body_GetMaxExtentOrigin( b3BodyId bodyId );
|
||||
|
||||
/// Get the closest point on a body to a world target.
|
||||
B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target );
|
||||
|
||||
/// Cast a ray at a specific body using a specified body transform.
|
||||
/// Ignores back-side collision on meshes and height-fields.
|
||||
B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter,
|
||||
float maxFraction, b3WorldTransform bodyTransform );
|
||||
|
||||
/// Cast a shape at a specific body using a specified body transform.
|
||||
/// Ignores back-side collision on meshes and height-fields.
|
||||
B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
|
||||
b3QueryFilter filter, float maxFraction, bool canEncroach,
|
||||
b3WorldTransform bodyTransform );
|
||||
|
|
@ -776,9 +790,17 @@ B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapePro
|
|||
b3WorldTransform bodyTransform );
|
||||
|
||||
/// Collide a character mover with a specific body using a specified body transform.
|
||||
/// Only considers convex shapes on the body.
|
||||
B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin,
|
||||
const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform );
|
||||
|
||||
/// Perform a time of impact between a character mover and a body using specified sweep transforms.
|
||||
/// Initial overlap of any shape on the body is ignored. A non-overlapped shape can still be hit.
|
||||
/// Only considers convex shapes on the body.
|
||||
B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation,
|
||||
b3QueryFilter filter, b3WorldTransform bodyTransform1,
|
||||
b3WorldTransform bodyTransform2 );
|
||||
|
||||
/** @} */ // body
|
||||
|
||||
/**
|
||||
|
|
@ -786,7 +808,7 @@ B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes,
|
|||
* Functions to create, destroy, and access.
|
||||
* Shapes bind raw geometry to bodies and hold material properties including friction and restitution.
|
||||
* You may add multiple shapes to a single body. There are no hard limits on shape count per body.
|
||||
*
|
||||
*
|
||||
* When you create a shape on a body the center of mass moves. This can lead to the body linear velocity
|
||||
* changing if the angular velocity is non-zero.
|
||||
* @{
|
||||
|
|
@ -1076,6 +1098,9 @@ B3_API void* b3Joint_GetUserData( b3JointId jointId );
|
|||
/// Wake the bodies connect to this joint
|
||||
B3_API void b3Joint_WakeBodies( b3JointId jointId );
|
||||
|
||||
/// Is the joint awake? If true then an attached body is awake.
|
||||
B3_API bool b3Joint_IsAwake( b3JointId jointId );
|
||||
|
||||
/// Get the current constraint force for this joint
|
||||
B3_API b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId );
|
||||
|
||||
|
|
@ -1552,7 +1577,9 @@ B3_API void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor );
|
|||
/// Is the spherical joint motor enabled?
|
||||
B3_API bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId );
|
||||
|
||||
/// Set the spherical joint motor velocity in radians per second
|
||||
/// Set the spherical joint motor velocity in radians per second. This is the relative angular
|
||||
/// velocity between the two bodies in world space.
|
||||
/// motorVelocity = angularVelocityB - angularVelocityA
|
||||
B3_API void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity );
|
||||
|
||||
/// Get the spherical joint motor velocity in radians per second
|
||||
|
|
|
|||
|
|
@ -137,3 +137,10 @@ B3_API float b3GetStallThreshold( void );
|
|||
#ifndef B3_RESTITUTION_ITERATIONS
|
||||
#define B3_RESTITUTION_ITERATIONS 1
|
||||
#endif
|
||||
|
||||
/// This is the limit on how many mesh or heightfield triangles a single convex shape can collide with.
|
||||
/// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data
|
||||
/// before increasing this. For example, using render mesh for collision often leads to poor performance.
|
||||
#ifndef B3_MAX_MESH_CONTACT_TRIANGLES
|
||||
#define B3_MAX_MESH_CONTACT_TRIANGLES 256
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -276,14 +276,12 @@ B3_INLINE float b3DistanceSquared( b3Vec3 a, b3Vec3 b )
|
|||
B3_FORCE_INLINE b3Vec3 b3Normalize( b3Vec3 a )
|
||||
{
|
||||
float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z;
|
||||
|
||||
if ( lengthSquared > 1000.0f * FLT_MIN )
|
||||
{
|
||||
float s = 1.0f / sqrtf( lengthSquared );
|
||||
b3Vec3 u = { s * a.x, s * a.y, s * a.z };
|
||||
return u;
|
||||
}
|
||||
|
||||
return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f };
|
||||
}
|
||||
|
||||
|
|
@ -291,15 +289,17 @@ B3_FORCE_INLINE b3Vec3 b3Normalize( b3Vec3 a )
|
|||
/// if the input is very small.
|
||||
B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 a )
|
||||
{
|
||||
*length = b3Length( a );
|
||||
if ( *length < FLT_EPSILON )
|
||||
float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z;
|
||||
if ( lengthSquared > 1000.0f * FLT_MIN )
|
||||
{
|
||||
return b3Vec3_zero;
|
||||
*length = sqrtf( lengthSquared );
|
||||
float s = 1.0f / *length;
|
||||
b3Vec3 u = { s * a.x, s * a.y, s * a.z };
|
||||
return u;
|
||||
}
|
||||
|
||||
float invLength = 1.0f / *length;
|
||||
b3Vec3 n = { invLength * a.x, invLength * a.y, invLength * a.z };
|
||||
return n;
|
||||
*length = 0.0f;
|
||||
return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f };
|
||||
}
|
||||
|
||||
/// Get a unit vector that is perpendicular to the supplied vector.
|
||||
|
|
@ -756,9 +756,10 @@ B3_INLINE b3WorldTransform b3MakeWorldTransform( b3Transform t )
|
|||
return w;
|
||||
}
|
||||
|
||||
/// Translate a local AABB by a world origin, rounding outward so the float box always contains
|
||||
/// the double box. Far from the origin a plain conversion could clip a shape out of its own box.
|
||||
/// In float mode the origin is float and the rounding is a no-op.
|
||||
/// Translate a local AABB by a world position, rounding outward so the single precision box
|
||||
/// encloses the double precision box. Far from the origin a plain conversion could clip a
|
||||
/// shape out of its own box.
|
||||
/// In single precision mode the the rounding is a no-op.
|
||||
B3_INLINE b3AABB b3OffsetAABB( b3AABB localBox, b3Pos origin )
|
||||
{
|
||||
b3AABB out;
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#include "id.h"
|
||||
#include "math_functions.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define B3_DEFAULT_CATEGORY_BITS UINT64_MAX
|
||||
|
|
@ -912,7 +913,9 @@ typedef struct b3SphericalJointDef
|
|||
/// The maximum motor torque, typically in newton-meters. Non-negative number.
|
||||
float maxMotorTorque;
|
||||
|
||||
/// The desired motor angular velocity in radians per second.
|
||||
/// The desired motor angular velocity in radians per second. This is the relative angular
|
||||
/// velocity between the two bodies in world space.
|
||||
/// motorVelocity = angularVelocityB - angularVelocityA
|
||||
b3Vec3 motorVelocity;
|
||||
} b3SphericalJointDef;
|
||||
|
||||
|
|
@ -1620,31 +1623,31 @@ typedef enum b3TOIState
|
|||
b3_toiStateSeparated
|
||||
} b3TOIState;
|
||||
|
||||
/// Time of impact output
|
||||
/// Time of impact output.
|
||||
typedef struct b3TOIOutput
|
||||
{
|
||||
/// The type of result
|
||||
/// The type of result.
|
||||
b3TOIState state;
|
||||
|
||||
/// The hit point
|
||||
/// The hit point. A shared point if overlapped.
|
||||
b3Vec3 point;
|
||||
|
||||
/// The hit normal
|
||||
/// The hit normal. Zero if overlapped.
|
||||
b3Vec3 normal;
|
||||
|
||||
/// The sweep time of the collision
|
||||
/// The sweep time of the collision. 0 if overlapped.
|
||||
float fraction;
|
||||
|
||||
/// The final distance
|
||||
/// The final distance. 0 if overlapped.
|
||||
float distance;
|
||||
|
||||
/// Number of outer iterations
|
||||
/// Number of outer iterations.
|
||||
int distanceIterations;
|
||||
|
||||
/// Total number of push back iterations
|
||||
/// Total number of push back iterations.
|
||||
int pushBackIterations;
|
||||
|
||||
/// Total number of root iterations
|
||||
/// Total number of root iterations.
|
||||
int rootIterations;
|
||||
|
||||
/// Indicates that the time of impact detected initial
|
||||
|
|
@ -1817,6 +1820,15 @@ typedef struct b3PlaneResult
|
|||
/// Closest point on the shape. May not be unique.
|
||||
b3Vec3 point;
|
||||
|
||||
/// The index of the mesh or height field triangle hit.
|
||||
int triangleIndex;
|
||||
|
||||
/// The index of the compound child shape.
|
||||
int childIndex;
|
||||
|
||||
/// The material index.
|
||||
int materialIndex;
|
||||
|
||||
} b3PlaneResult;
|
||||
|
||||
/// These are collision planes that can be fed to b3SolvePlanes. Normally
|
||||
|
|
@ -1857,6 +1869,22 @@ typedef struct b3BodyPlaneResult
|
|||
b3PlaneResult result;
|
||||
} b3BodyPlaneResult;
|
||||
|
||||
/// Body time of impact result for movers.
|
||||
typedef struct b3BodyTOIResult
|
||||
{
|
||||
/// The hit point in world space.
|
||||
b3Pos point;
|
||||
|
||||
/// The hit normal. Points from the body to the mover.
|
||||
b3Vec3 normal;
|
||||
|
||||
/// The sweep time of the collision.
|
||||
float fraction;
|
||||
|
||||
/// The hit shape.
|
||||
b3ShapeId shapeId;
|
||||
} b3BodyTOIResult;
|
||||
|
||||
/// Used to collect collision planes for character movers.
|
||||
/// Return true to continue gathering planes.
|
||||
typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, void* context );
|
||||
|
|
@ -2068,7 +2096,10 @@ typedef struct b3MeshDef
|
|||
/// Triangle vertices.
|
||||
b3Vec3* vertices;
|
||||
|
||||
/// Triangle vertex indices. 3 for each triangle. CCW winding.
|
||||
/// Stride between vertices. Use 0 for contiguous vertices.
|
||||
size_t stride;
|
||||
|
||||
/// Triangle vertex indices. 3 for each triangle. CCW winding unless CW is indicated below.
|
||||
int32_t* indices;
|
||||
|
||||
/// Triangle material index. 1 per triangle. Indexes into b3ShapeDef::materials.
|
||||
|
|
@ -2094,6 +2125,9 @@ typedef struct b3MeshDef
|
|||
|
||||
/// Compute triangle adjacency information using shared edges
|
||||
bool identifyEdges;
|
||||
|
||||
/// Input indices have clockWise winding order.
|
||||
bool clockWiseWinding;
|
||||
} b3MeshDef;
|
||||
|
||||
/// 64-bit mesh version. Useful for validating serialized data.
|
||||
|
|
|
|||
|
|
@ -221,6 +221,12 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
|
|||
continue;
|
||||
}
|
||||
|
||||
MoverShapeUserData* userData = static_cast<MoverShapeUserData*>( b3Shape_GetUserData( m_planeExtras[i].shapeId ) );
|
||||
if ( userData != nullptr && userData->canMoverPush == false )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Pos point = m_planeExtras[i].point;
|
||||
b3Vec3 normal = b3Neg( m_planes[i].plane.normal );
|
||||
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@ struct MoverShapeUserData
|
|||
{
|
||||
float maxPush;
|
||||
bool clipVelocity;
|
||||
bool canMoverPush;
|
||||
};
|
||||
|
||||
struct PlaneExtra
|
||||
|
|
|
|||
|
|
@ -312,10 +312,19 @@ public:
|
|||
|
||||
static int sampleMoverOverlap = RegisterSample( "Character", "MoverOverlap", MoverOverlap::Create );
|
||||
|
||||
class BasicMover : public Sample
|
||||
// What the mover was last struck with. Stays on screen until the next hit.
|
||||
struct MoverImpact
|
||||
{
|
||||
b3Pos point;
|
||||
b3Vec3 normal;
|
||||
float speed;
|
||||
bool valid;
|
||||
};
|
||||
|
||||
class GeometricMover : public Sample
|
||||
{
|
||||
public:
|
||||
explicit BasicMover( SampleContext* context )
|
||||
explicit GeometricMover( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
b3Pos moverPosition = { 7.5f, 0.75f, 9.0f };
|
||||
|
|
@ -396,6 +405,19 @@ public:
|
|||
b3CreateHeightFieldShape( body, &shapeDef, m_heightField );
|
||||
}
|
||||
|
||||
// The tree is longer than the height field is wide, so topple it along the diagonal.
|
||||
{
|
||||
b3Vec3 axis = { 1.0f, 0.0f, -1.0f };
|
||||
m_treeSpin = 0.5f * b3Normalize( axis );
|
||||
m_treeSpawn = { 22.0f, 1.5f, 2.0f };
|
||||
m_treeTimer = 0.0f;
|
||||
m_treeShapeUserData.maxPush = 10.0f;
|
||||
m_treeShapeUserData.clipVelocity = true;
|
||||
m_treeShapeUserData.canMoverPush = false;
|
||||
m_treeImpact = {};
|
||||
CreateFallingTree();
|
||||
}
|
||||
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.position = { 0.0f, 1.4f, 6.0f };
|
||||
|
|
@ -403,6 +425,7 @@ public:
|
|||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
m_enemyShape.maxPush = 1.0f;
|
||||
m_enemyShape.clipVelocity = true;
|
||||
m_enemyShape.canMoverPush = false;
|
||||
|
||||
b3Capsule capsule = {
|
||||
{ 0.0f, -0.5f, 0.0f },
|
||||
|
|
@ -424,6 +447,7 @@ public:
|
|||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
m_friendlyShape.maxPush = 0.01f;
|
||||
m_friendlyShape.clipVelocity = false;
|
||||
m_friendlyShape.canMoverPush = false;
|
||||
|
||||
b3Capsule capsule = {
|
||||
{ 0.0f, -0.5f, 0.0f },
|
||||
|
|
@ -449,6 +473,28 @@ public:
|
|||
b3CreateSphereShape( body, &shapeDef, &sphere );
|
||||
}
|
||||
|
||||
// Falls through the mover start position on a loop to feed the mover time of impact.
|
||||
{
|
||||
m_fallingExtent = { 1.0f, 0.1, 1.0f };
|
||||
m_fallingSpawn = { 10.5f, 30.0f, 0.0f };
|
||||
m_fallingSpin = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = m_fallingSpawn;
|
||||
bodyDef.angularVelocity = m_fallingSpin;
|
||||
m_fallingBoxId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.customColor = b3_colorGold;
|
||||
b3BoxHull box = b3MakeBoxHull( m_fallingExtent.x, m_fallingExtent.y, m_fallingExtent.z );
|
||||
b3CreateHullShape( m_fallingBoxId, &shapeDef, &box.base );
|
||||
|
||||
m_respawnTimer = 0.0f;
|
||||
m_impactTransform = { b3Pos_zero, b3Quat_identity };
|
||||
m_boxImpact = {};
|
||||
}
|
||||
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.position = { 7.0f, 2.0f, -3.0f };
|
||||
|
|
@ -519,7 +565,7 @@ public:
|
|||
// m_mouseDelta = { 0.0f, 0.0f };
|
||||
}
|
||||
|
||||
~BasicMover() override
|
||||
~GeometricMover() override
|
||||
{
|
||||
m_camera->m_thirdPerson = false;
|
||||
sapp_lock_mouse( false );
|
||||
|
|
@ -556,32 +602,201 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
// One tree from the falling trees benchmark: a stack of shrinking cylinders sharing a body.
|
||||
void CreateFallingTree()
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = m_treeSpawn;
|
||||
bodyDef.sleepThreshold = 0.2f;
|
||||
m_treeId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.9f;
|
||||
shapeDef.baseMaterial.rollingResistance = 0.05f;
|
||||
shapeDef.updateBodyMass = false;
|
||||
shapeDef.userData = &m_treeShapeUserData;
|
||||
|
||||
float y = 1.0f;
|
||||
float r = 0.75f;
|
||||
float l = 1.5f;
|
||||
for ( int i = 0; i < 22; ++i )
|
||||
{
|
||||
b3HullData* hull = b3CreateCylinder( l + 2.0f * r, r, y - r, 6 );
|
||||
b3CreateHullShape( m_treeId, &shapeDef, hull );
|
||||
b3DestroyHull( hull );
|
||||
|
||||
y += l + 2.0f * r;
|
||||
r = 0.95f * r;
|
||||
}
|
||||
|
||||
b3Body_ApplyMassFromShapes( m_treeId );
|
||||
|
||||
b3Pos center = b3Body_GetWorldCenter( m_treeId );
|
||||
b3Body_SetAngularVelocity( m_treeId, m_treeSpin );
|
||||
b3Body_SetLinearVelocity( m_treeId, b3Cross( m_treeSpin, b3SubPos( center, m_treeSpawn ) ) );
|
||||
}
|
||||
|
||||
// Spin about the base so the trunk sweeps down through anything standing in the fall line.
|
||||
void ResetFallingTree()
|
||||
{
|
||||
b3DestroyBody( m_treeId );
|
||||
CreateFallingTree();
|
||||
}
|
||||
|
||||
// The normal points from the body to the mover, so approach gives a positive speed.
|
||||
MoverImpact MakeImpact( b3BodyId bodyId, const b3BodyTOIResult* toi )
|
||||
{
|
||||
b3Vec3 pointVelocity = b3Body_GetWorldPointVelocity( bodyId, toi->point );
|
||||
float speed = b3Dot( toi->normal, b3Sub( pointVelocity, m_mover.m_velocity ) );
|
||||
|
||||
return { toi->point, toi->normal, speed, true };
|
||||
}
|
||||
|
||||
void DrawImpact( const MoverImpact* impact, b3HexColor hexColor )
|
||||
{
|
||||
if ( impact->valid == false )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Vec4 color = MakeColor( hexColor );
|
||||
b3Pos tip = impact->point + 0.5f * impact->normal;
|
||||
|
||||
DrawPoint( impact->point, 6.0f, color );
|
||||
DrawArrow( impact->point, tip, color );
|
||||
DrawString3D( tip, color, " %.1f m/s", impact->speed );
|
||||
}
|
||||
|
||||
// True once the body has held still for a second.
|
||||
bool SettledForOneSecond( b3BodyId bodyId, float* timer )
|
||||
{
|
||||
if ( b3Body_IsAwake( bodyId ) )
|
||||
{
|
||||
*timer = 0.0f;
|
||||
return false;
|
||||
}
|
||||
|
||||
*timer += m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f;
|
||||
if ( *timer < 1.0f )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
*timer = 0.0f;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Same sweep as the box, but the tree reports what the character was struck with.
|
||||
void UpdateFallingTree( b3Pos moverStart, b3WorldTransform treeStart )
|
||||
{
|
||||
b3WorldTransform treeEnd = b3Body_GetTransform( m_treeId );
|
||||
b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart );
|
||||
|
||||
b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_treeId, moverStart, &m_mover.m_capsule, moverTranslation,
|
||||
b3DefaultQueryFilter(), treeStart, treeEnd );
|
||||
|
||||
if ( toi.fraction > 0.0f )
|
||||
{
|
||||
m_treeImpact = MakeImpact( m_treeId, &toi );
|
||||
}
|
||||
|
||||
if ( SettledForOneSecond( m_treeId, &m_treeTimer ) )
|
||||
{
|
||||
ResetFallingTree();
|
||||
}
|
||||
}
|
||||
|
||||
// Sweep the mover capsule against the box motion from this step and keep the box pose at first
|
||||
// touch. Both sweeps cover the same interval, so the mover translation comes from this step too.
|
||||
void UpdateFallingBox( b3Pos moverStart, b3WorldTransform boxStart )
|
||||
{
|
||||
b3WorldTransform boxEnd = b3Body_GetTransform( m_fallingBoxId );
|
||||
b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart );
|
||||
|
||||
b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_fallingBoxId, moverStart, &m_mover.m_capsule, moverTranslation,
|
||||
b3DefaultQueryFilter(), boxStart, boxEnd );
|
||||
|
||||
// A zero fraction means the box was already resting against the mover, so keep the pose from
|
||||
// the step that first reached it.
|
||||
if ( toi.fraction > 0.0f )
|
||||
{
|
||||
m_impactTransform.p = boxStart.p + toi.fraction * b3SubPos( boxEnd.p, boxStart.p );
|
||||
m_impactTransform.q = b3NLerp( boxStart.q, boxEnd.q, toi.fraction );
|
||||
m_boxImpact = MakeImpact( m_fallingBoxId, &toi );
|
||||
}
|
||||
|
||||
if ( SettledForOneSecond( m_fallingBoxId, &m_respawnTimer ) )
|
||||
{
|
||||
RespawnFallingBox();
|
||||
}
|
||||
}
|
||||
|
||||
void RespawnFallingBox()
|
||||
{
|
||||
b3Body_SetTransform( m_fallingBoxId, m_fallingSpawn, b3Quat_identity );
|
||||
b3Body_SetLinearVelocity( m_fallingBoxId, b3Vec3_zero );
|
||||
b3Body_SetAngularVelocity( m_fallingBoxId, m_fallingSpin );
|
||||
b3Body_SetAwake( m_fallingBoxId, true );
|
||||
}
|
||||
|
||||
void Step() override
|
||||
{
|
||||
b3Pos moverStart = m_mover.m_transform.p;
|
||||
b3WorldTransform boxStart = b3Body_GetTransform( m_fallingBoxId );
|
||||
b3WorldTransform treeStart = b3Body_GetTransform( m_treeId );
|
||||
|
||||
m_mover.Step( &m_ignoreShapeId, 1, m_clipVelocity );
|
||||
DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson );
|
||||
DrawTextLine( "deltaX = %g, deltaY = %g", m_mouseDelta.x, m_mouseDelta.y );
|
||||
|
||||
Sample::Step();
|
||||
|
||||
if ( m_didStep )
|
||||
{
|
||||
UpdateFallingBox( moverStart, boxStart );
|
||||
UpdateFallingTree( moverStart, treeStart );
|
||||
}
|
||||
|
||||
if ( m_boxImpact.valid )
|
||||
{
|
||||
DrawCube( m_impactTransform, 2.0f * m_fallingExtent, MakeColorAlpha( b3_colorYellow, 0.4f ) );
|
||||
}
|
||||
|
||||
DrawImpact( &m_boxImpact, b3_colorYellow );
|
||||
DrawImpact( &m_treeImpact, b3_colorCyan );
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new BasicMover( context );
|
||||
return new GeometricMover( context );
|
||||
}
|
||||
|
||||
CharacterMover m_mover;
|
||||
MoverShapeUserData m_enemyShape;
|
||||
MoverShapeUserData m_friendlyShape;
|
||||
MoverShapeUserData m_treeShapeUserData;
|
||||
b3MeshData* m_levelMesh;
|
||||
b3MeshData* m_stairs;
|
||||
b3MeshData* m_torus;
|
||||
b3HeightFieldData* m_heightField;
|
||||
b3ShapeId m_ignoreShapeId;
|
||||
b3BodyId m_fallingBoxId;
|
||||
b3BodyId m_treeId;
|
||||
b3WorldTransform m_impactTransform;
|
||||
MoverImpact m_boxImpact;
|
||||
MoverImpact m_treeImpact;
|
||||
b3Pos m_treeSpawn;
|
||||
b3Vec3 m_treeSpin;
|
||||
float m_treeTimer;
|
||||
b3Pos m_fallingSpawn;
|
||||
b3Vec3 m_fallingExtent;
|
||||
b3Vec3 m_fallingSpin;
|
||||
float m_respawnTimer;
|
||||
bool m_clipVelocity;
|
||||
};
|
||||
|
||||
static int sampleMover = RegisterSample( "Character", "Mover", BasicMover::Create );
|
||||
static int sampleMover = RegisterSample( "Character", "Geometric Mover", GeometricMover::Create );
|
||||
|
||||
struct ClosestShapeCastContext
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1302,3 +1302,87 @@ public:
|
|||
|
||||
static int sampleSlideTwistOffCenterShape =
|
||||
RegisterSample( "Issues", "Slide Twist Off Center Shape", SlideTwistOffCenterShape::Create );
|
||||
|
||||
// Very large dynamic bodies have overflow when computing the inertia tensor.
|
||||
class HugeBox : public Sample
|
||||
{
|
||||
public:
|
||||
explicit HugeBox( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Pos_zero );
|
||||
}
|
||||
|
||||
AddGroundBox( 400.0f );
|
||||
|
||||
{
|
||||
float a = 100.0f;
|
||||
|
||||
b3BoxHull cube = b3MakeCubeHull( a );
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.name = "cube";
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { 0.0f, a, 0.0f };
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.2f );
|
||||
m_bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( m_bodyId, &shapeDef, &cube.base );
|
||||
}
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new HugeBox( context );
|
||||
}
|
||||
|
||||
b3BodyId m_bodyId;
|
||||
};
|
||||
|
||||
static int sampleHugeBox = RegisterSample( "Issues", "Huge Box", HugeBox::Create );
|
||||
|
||||
// This demonstrates this cap on mesh complexity. B3_MAX_MESH_CONTACT_TRIANGLES
|
||||
// It generates this warning:
|
||||
// Box3D: WARNING: complex mesh detected, triangle buffer capacity of 256 reached
|
||||
class HeightfieldIssue : public Sample
|
||||
{
|
||||
public:
|
||||
explicit HeightfieldIssue( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( -90.0f, 5.0f, 50.0f, { 50.0f, 10.0f, 50.0f } );
|
||||
}
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
|
||||
m_heightField = b3CreateGrid( 100, 100, { 1, 1, 1 }, false );
|
||||
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHeightFieldShape( groundId, &shapeDef, m_heightField );
|
||||
|
||||
b3HullData* hull = b3CreateCylinder( 20, 5, -10, 24 );
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { 50, 20, 50 };
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, B3_PI / 4 );
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( bodyId, &shapeDef, hull );
|
||||
b3DestroyHull( hull );
|
||||
}
|
||||
|
||||
~HeightfieldIssue() override
|
||||
{
|
||||
b3DestroyHeightField( m_heightField );
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new HeightfieldIssue( context );
|
||||
}
|
||||
|
||||
b3HeightFieldData* m_heightField;
|
||||
};
|
||||
static int sampleIssueIndex = RegisterSample( "Issues", "Heightfield", HeightfieldIssue::Create );
|
||||
|
|
|
|||
|
|
@ -1953,6 +1953,67 @@ public:
|
|||
|
||||
static int sampleBridgeIndex = RegisterSample( "Joints", "Bridge", Bridge::Create );
|
||||
|
||||
class FoldOut : public Sample
|
||||
{
|
||||
public:
|
||||
explicit FoldOut( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 20.0f, 10.0f, 28.0f, { 0.0f, 7.0f, 0.0f } );
|
||||
}
|
||||
|
||||
float halfLength = 2.0f;
|
||||
float spacing = 1.0f;
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( halfLength, 0.1f, 0.25f );
|
||||
|
||||
b3Quat axisQuat = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY );
|
||||
|
||||
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
|
||||
jointDef.base.localFrameA.p = { -halfLength, 1.0f, 0.0f };
|
||||
jointDef.base.localFrameA.q = axisQuat;
|
||||
jointDef.base.localFrameB.q = axisQuat;
|
||||
jointDef.base.drawScale = 0.5f;
|
||||
jointDef.base.constraintHertz = 240.0f;
|
||||
jointDef.enableMotor = true;
|
||||
jointDef.maxMotorTorque = 1000.0f;
|
||||
|
||||
b3BodyId prevBodyId = AddGroundBox( 20.0f );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.position.y = 0.5f * spacing;
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.density *= 2.0f;
|
||||
|
||||
for ( int i = 0; i < 6; ++i )
|
||||
{
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
float x = ( i & 1 ) ? halfLength : -halfLength;
|
||||
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = bodyId;
|
||||
jointDef.base.localFrameB.p = { x, -0.5f * spacing, 0.0f };
|
||||
b3CreateRevoluteJoint( m_worldId, &jointDef );
|
||||
|
||||
bodyDef.position.y += spacing;
|
||||
jointDef.base.localFrameA.p = { -x, 0.5f * spacing, 0.0f };
|
||||
prevBodyId = bodyId;
|
||||
}
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new FoldOut( context );
|
||||
}
|
||||
};
|
||||
|
||||
static int sampleFoldOut = RegisterSample( "Joints", "Fold Out", FoldOut::Create );
|
||||
|
||||
// This test ensures joints work correctly with bodies that have motion locks
|
||||
class MotionLocks : public Sample
|
||||
{
|
||||
|
|
@ -2000,6 +2061,8 @@ public:
|
|||
assert( m_count < m_capacity );
|
||||
|
||||
bodyDef.position = position;
|
||||
bodyDef.angularDamping = 0.5f;
|
||||
|
||||
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
|
||||
|
||||
|
|
@ -2403,7 +2466,7 @@ public:
|
|||
|
||||
~Driving() override
|
||||
{
|
||||
if (m_camera->m_thirdPerson)
|
||||
if ( m_camera->m_thirdPerson )
|
||||
{
|
||||
m_camera->m_thirdPerson = false;
|
||||
sapp_lock_mouse( false );
|
||||
|
|
|
|||
109
src/body.c
109
src/body.c
|
|
@ -779,6 +779,87 @@ int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int pla
|
|||
return resultCount;
|
||||
}
|
||||
|
||||
b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation,
|
||||
b3QueryFilter filter, b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 )
|
||||
{
|
||||
b3BodyTOIResult result = { 0 };
|
||||
result.fraction = 1.0f;
|
||||
|
||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||
if ( world == NULL )
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
b3Transform xf1 = b3ToRelativeTransform( bodyTransform1, origin );
|
||||
b3Transform xf2 = b3ToRelativeTransform( bodyTransform2, origin );
|
||||
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
b3Vec3 localCenter = bodySim->localCenter;
|
||||
|
||||
b3Vec3 capsulePoints[2] = { mover->center1, mover->center2 };
|
||||
b3TOIInput input = { 0 };
|
||||
input.proxyB = (b3ShapeProxy){
|
||||
.points = capsulePoints,
|
||||
.count = 2,
|
||||
.radius = mover->radius,
|
||||
};
|
||||
input.sweepA.c1 = b3TransformPoint( xf1, localCenter );
|
||||
input.sweepA.c2 = b3TransformPoint( xf2, localCenter );
|
||||
input.sweepA.q1 = bodyTransform1.q;
|
||||
input.sweepA.q2 = bodyTransform2.q;
|
||||
input.sweepA.localCenter = localCenter;
|
||||
|
||||
input.sweepB.c1 = b3Vec3_zero;
|
||||
input.sweepB.c2 = moverTranslation;
|
||||
input.sweepB.q1 = b3Quat_identity;
|
||||
input.sweepB.q2 = b3Quat_identity;
|
||||
input.sweepB.localCenter = b3Vec3_zero;
|
||||
|
||||
input.maxFraction = 1.0f;
|
||||
|
||||
int shapeId = body->headShapeId;
|
||||
while ( shapeId != B3_NULL_INDEX )
|
||||
{
|
||||
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
|
||||
shapeId = shape->nextShapeId;
|
||||
|
||||
if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3ShapeType type = shape->type;
|
||||
if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
input.proxyA = b3MakeShapeProxy( shape );
|
||||
|
||||
b3TOIOutput output = b3TimeOfImpact( &input );
|
||||
B3_VALIDATE( output.state != b3_toiStateUnknown );
|
||||
|
||||
// Mimic behavior in b3ContinuousQueryCallback. Ignore shapes that initially overlap.
|
||||
if (0.0f < output.fraction && output.fraction < result.fraction)
|
||||
{
|
||||
input.maxFraction = output.fraction;
|
||||
|
||||
result.point = b3OffsetPos( origin, output.point );
|
||||
result.normal = output.normal;
|
||||
result.fraction = output.fraction;
|
||||
result.shapeId = (b3ShapeId){
|
||||
.index1 = shape->id + 1,
|
||||
.world0 = world->worldId,
|
||||
.generation = shape->generation,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
||||
{
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
|
|
@ -2303,7 +2384,7 @@ bool b3Body_IsBullet( b3BodyId bodyId )
|
|||
return ( body->flags & b3_isBullet ) != 0;
|
||||
}
|
||||
|
||||
void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag)
|
||||
void b3Body_AllowFastRotation( b3BodyId bodyId, bool flag )
|
||||
{
|
||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||
if ( world == NULL )
|
||||
|
|
@ -2327,7 +2408,7 @@ void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag)
|
|||
b3SyncBodyFlags( world, body );
|
||||
}
|
||||
|
||||
bool b3Body_IsFastRotationAllowed(b3BodyId bodyId)
|
||||
bool b3Body_IsFastRotationAllowed( b3BodyId bodyId )
|
||||
{
|
||||
b3World* world = b3GetWorld( bodyId.world0 );
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
|
|
@ -2481,3 +2562,27 @@ bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB )
|
|||
|
||||
return true;
|
||||
}
|
||||
|
||||
float b3Body_GetMinExtent( b3BodyId bodyId )
|
||||
{
|
||||
b3World* world = b3GetWorld( bodyId.world0 );
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
return bodySim->minExtent;
|
||||
}
|
||||
|
||||
b3Vec3 b3Body_GetMaxExtent( b3BodyId bodyId )
|
||||
{
|
||||
b3World* world = b3GetWorld( bodyId.world0 );
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
return bodySim->maxExtent;
|
||||
}
|
||||
|
||||
b3Vec3 b3Body_GetMaxExtentOrigin( b3BodyId bodyId )
|
||||
{
|
||||
b3World* world = b3GetWorld( bodyId.world0 );
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
return b3Add( bodySim->maxExtent, b3Abs( bodySim->localCenter ) );
|
||||
}
|
||||
|
|
|
|||
|
|
@ -380,6 +380,6 @@ int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, con
|
|||
}
|
||||
|
||||
b3Plane plane = { normal, totalRadius - distance };
|
||||
*result = (b3PlaneResult){ plane, approach.point1 };
|
||||
*result = (b3PlaneResult){ plane, approach.point1, 0, 0, 0 };
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -399,7 +399,9 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
// No effort to share mesh materials. It would be easier to do if the number of materials was limited.
|
||||
B3_ASSERT( meshData->materialCount == meshDef->materialCount );
|
||||
|
||||
for ( int j = 0; j < meshDef->materialCount; ++j )
|
||||
int meshMaterialCount = b3MinInt( meshDef->materialCount, B3_MAX_COMPOUND_MESH_MATERIALS );
|
||||
|
||||
for ( int j = 0; j < meshMaterialCount; ++j )
|
||||
{
|
||||
// Look for an existing material.
|
||||
b3MaterialMap_itr materialItr =
|
||||
|
|
@ -1168,6 +1170,10 @@ static bool b3CompoundMoverCallback( int proxyId, uint64_t userData, void* conte
|
|||
{
|
||||
planes[i].plane.normal = b3RotateVector( child.transform.q, planes[i].plane.normal );
|
||||
planes[i].point = b3TransformPoint( child.transform, planes[i].point );
|
||||
|
||||
planes[i].childIndex = childIndex;
|
||||
int childMaterialIndex = b3MinInt( planes[i].materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 );
|
||||
planes[i].materialIndex = child.materialIndices[childMaterialIndex];
|
||||
}
|
||||
|
||||
moverContext->planeCount += planeCount;
|
||||
|
|
|
|||
|
|
@ -278,7 +278,6 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L
|
|||
// Step 2: Find farthest point in 2D
|
||||
bestScore = 0.0f;
|
||||
bestIndex = B3_NULL_INDEX;
|
||||
float maxDistanceSquared = 0.0f;
|
||||
|
||||
for ( int index = 0; index < count; ++index )
|
||||
{
|
||||
|
|
@ -286,7 +285,6 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L
|
|||
b3Vec3 d = b3Sub( p, a );
|
||||
b3Vec3 v = b3MulSub( d, b3Dot( d, normal ), normal );
|
||||
float distanceSquared = b3LengthSquared( v );
|
||||
maxDistanceSquared = b3MaxFloat( maxDistanceSquared, distanceSquared );
|
||||
float separation = b3MaxFloat( 0.0f, -points[index].separation );
|
||||
float score = distanceSquared + 4.0f * separation * separation;
|
||||
if ( bias * score > bestScore )
|
||||
|
|
|
|||
|
|
@ -847,6 +847,10 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
|
|||
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
|
||||
distanceOutput.pointA = localPointA;
|
||||
distanceOutput.pointB = localPointB;
|
||||
distanceOutput.normal = b3Vec3_zero;
|
||||
distanceOutput.distance = 0.0f;
|
||||
distanceOutput.iterations = iteration;
|
||||
distanceOutput.simplexCount = simplexIndex;
|
||||
return distanceOutput;
|
||||
}
|
||||
|
||||
|
|
@ -943,6 +947,10 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
|
|||
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
|
||||
distanceOutput.pointA = localPointA;
|
||||
distanceOutput.pointB = localPointB;
|
||||
distanceOutput.normal = b3Vec3_zero;
|
||||
distanceOutput.distance = 0.0f;
|
||||
distanceOutput.iterations = iteration;
|
||||
distanceOutput.simplexCount = simplexIndex;
|
||||
B3_VALIDATE( b3Distance( localPointA, localPointB ) < FLT_EPSILON );
|
||||
return distanceOutput;
|
||||
}
|
||||
|
|
@ -984,25 +992,27 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
|
|||
simplex.count += 1;
|
||||
}
|
||||
|
||||
// Build witness points.
|
||||
b3Vec3 localPointA, localPointB;
|
||||
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
|
||||
|
||||
distanceOutput.pointA = localPointA;
|
||||
distanceOutput.pointB = localPointB;
|
||||
distanceOutput.iterations = iteration;
|
||||
distanceOutput.simplexCount = simplexIndex;
|
||||
|
||||
normal = b3Normalize( normal );
|
||||
if ( b3IsNormalized( normal ) == false )
|
||||
{
|
||||
// Treat as overlap
|
||||
distanceOutput.distance = 0.0f;
|
||||
distanceOutput.normal = b3Vec3_zero;
|
||||
return distanceOutput;
|
||||
}
|
||||
|
||||
// Build witness points and safe cache
|
||||
b3Vec3 localPointA, localPointB;
|
||||
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
|
||||
b3WriteCache( cache, &simplex );
|
||||
|
||||
// Results stay in frame A
|
||||
distanceOutput.pointA = localPointA;
|
||||
distanceOutput.pointB = localPointB;
|
||||
distanceOutput.distance = b3Distance( localPointA, localPointB );
|
||||
distanceOutput.normal = normal;
|
||||
distanceOutput.iterations = iteration;
|
||||
distanceOutput.simplexCount = simplexIndex;
|
||||
|
||||
// Apply radii if requested
|
||||
if ( input->useRadii )
|
||||
|
|
@ -1016,10 +1026,12 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
|
|||
distanceOutput.pointB = b3Sub( distanceOutput.pointB, b3MulSV( rB, normal ) );
|
||||
}
|
||||
|
||||
// Simplex is useful, cache it.
|
||||
b3WriteCache( cache, &simplex );
|
||||
|
||||
return distanceOutput;
|
||||
}
|
||||
|
||||
|
||||
// Separation function:
|
||||
// f(t) = (c2 + t * dp2 - c1 - t * dp1 ) * n
|
||||
|
||||
|
|
@ -1703,6 +1715,14 @@ b3TOIOutput b3TimeOfImpact( const b3TOIInput* input )
|
|||
{
|
||||
output.state = b3_toiStateOverlapped;
|
||||
output.fraction = 0.0f;
|
||||
|
||||
// Averaged hit point
|
||||
b3Vec3 pA = b3MulAdd( worldPointA, proxyA.radius, worldNormal );
|
||||
b3Vec3 pB = b3MulAdd( worldPointB, -proxyB.radius, worldNormal );
|
||||
output.point = b3Lerp( pA, pB, 0.5f );
|
||||
output.point = b3Add( output.point, origin );
|
||||
output.normal = worldNormal;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -608,6 +608,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
b3Vec3 shapeTranslation = input->translation;
|
||||
b3Vec3 scale = heightField->scale;
|
||||
|
||||
// The shape start is the center of the proxy.
|
||||
b3Vec3 shapeStart = b3AABB_Center( shapeBounds );
|
||||
b3Vec3 shapeDelta = b3MulSV( input->maxFraction, shapeTranslation );
|
||||
b3Vec3 shapeEnd = b3Add( shapeStart, shapeDelta );
|
||||
|
|
@ -768,9 +769,12 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
castBounds.lowerBound = b3Sub( b3Min( centerStart, centerEnd ), shapeExtents );
|
||||
castBounds.upperBound = b3Add( b3Max( centerStart, centerEnd ), shapeExtents );
|
||||
|
||||
// The ray origin is the center of the shape.
|
||||
b3V32 rayOrigin = b3LoadV( &shapeStart.x );
|
||||
b3V32 rayTranslation = b3LoadV( &shapeTranslation.x );
|
||||
|
||||
bool clockwise = heightField->clockwise;
|
||||
|
||||
while ( true )
|
||||
{
|
||||
int column1, column2;
|
||||
|
|
@ -827,6 +831,11 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
b3Vec3 point21 = corners[2];
|
||||
b3Vec3 point22 = corners[3];
|
||||
|
||||
if ( clockwise )
|
||||
{
|
||||
B3_SWAP( point12, point21 );
|
||||
}
|
||||
|
||||
// I know the min/max x and z values, but not the min/max heights.
|
||||
b3AABB bounds;
|
||||
bounds.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) );
|
||||
|
|
@ -846,18 +855,8 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
// Ray cast
|
||||
{
|
||||
b3V32 vertex1 = b3LoadV( &point11.x );
|
||||
b3V32 vertex2, vertex3;
|
||||
|
||||
if ( heightField->clockwise )
|
||||
{
|
||||
vertex2 = b3LoadV( &point12.x );
|
||||
vertex3 = b3LoadV( &point21.x );
|
||||
}
|
||||
else
|
||||
{
|
||||
vertex2 = b3LoadV( &point21.x );
|
||||
vertex3 = b3LoadV( &point12.x );
|
||||
}
|
||||
b3V32 vertex2 = b3LoadV( &point21.x );
|
||||
b3V32 vertex3 = b3LoadV( &point12.x );
|
||||
|
||||
float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 );
|
||||
B3_ASSERT( 0 <= alpha && alpha <= 1.0f );
|
||||
|
|
@ -866,7 +865,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
{
|
||||
b3Vec3 edge1 = b3Sub( point21, point11 );
|
||||
b3Vec3 edge2 = b3Sub( point12, point11 );
|
||||
b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 );
|
||||
b3Vec3 normal = b3Cross( edge1, edge2 );
|
||||
|
||||
result.point = b3MulAdd( shapeStart, alpha, shapeTranslation );
|
||||
result.normal = b3Normalize( normal );
|
||||
|
|
@ -880,18 +879,8 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
|
||||
{
|
||||
b3V32 vertex1 = b3LoadV( &point22.x );
|
||||
b3V32 vertex2, vertex3;
|
||||
|
||||
if ( heightField->clockwise )
|
||||
{
|
||||
vertex2 = b3LoadV( &point21.x );
|
||||
vertex3 = b3LoadV( &point12.x );
|
||||
}
|
||||
else
|
||||
{
|
||||
vertex2 = b3LoadV( &point12.x );
|
||||
vertex3 = b3LoadV( &point21.x );
|
||||
}
|
||||
b3V32 vertex2 = b3LoadV( &point12.x );
|
||||
b3V32 vertex3 = b3LoadV( &point21.x );
|
||||
|
||||
float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 );
|
||||
B3_ASSERT( 0 <= alpha && alpha <= 1.0f );
|
||||
|
|
@ -900,7 +889,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
{
|
||||
b3Vec3 edge1 = b3Sub( point22, point21 );
|
||||
b3Vec3 edge2 = b3Sub( point12, point21 );
|
||||
b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 );
|
||||
b3Vec3 normal = b3Cross( edge1, edge2 );
|
||||
|
||||
result.point = b3MulAdd( shapeStart, alpha, shapeTranslation );
|
||||
result.normal = b3Normalize( normal );
|
||||
|
|
@ -915,44 +904,55 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const
|
|||
else
|
||||
{
|
||||
// Shape cast
|
||||
// todo back-side culling
|
||||
{
|
||||
// Shift origin to first vertex
|
||||
b3Vec3 origin = point11;
|
||||
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) };
|
||||
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
pairInput.maxFraction = bestFraction;
|
||||
pairInput.transform.p = b3Neg( origin );
|
||||
// shapeStart is the center of the proxy.
|
||||
float signedVolume = b3SignedVolume( point11, point21, point12, shapeStart );
|
||||
|
||||
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
|
||||
|
||||
if ( pairOutput.hit )
|
||||
if ( signedVolume >= 0.0f )
|
||||
{
|
||||
bestFraction = pairOutput.fraction;
|
||||
result = pairOutput;
|
||||
result.point = b3Add( result.point, origin );
|
||||
result.triangleIndex = triangleIndex1;
|
||||
result.materialIndex = materialIndex;
|
||||
// Shift origin to first vertex
|
||||
b3Vec3 origin = point11;
|
||||
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) };
|
||||
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
pairInput.maxFraction = bestFraction;
|
||||
pairInput.transform.p = b3Neg( origin );
|
||||
|
||||
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
|
||||
|
||||
if ( pairOutput.hit )
|
||||
{
|
||||
bestFraction = pairOutput.fraction;
|
||||
result = pairOutput;
|
||||
result.point = b3Add( result.point, origin );
|
||||
result.triangleIndex = triangleIndex1;
|
||||
result.materialIndex = materialIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
// Shift origin to first vertex
|
||||
b3Vec3 origin = point21;
|
||||
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) };
|
||||
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
pairInput.maxFraction = bestFraction;
|
||||
pairInput.transform.p = b3Neg( origin );
|
||||
// shapeStart is the center of the proxy.
|
||||
float signedVolume = b3SignedVolume( point21, point22, point12, shapeStart );
|
||||
|
||||
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
|
||||
|
||||
if ( pairOutput.hit )
|
||||
if ( signedVolume >= 0.0f )
|
||||
{
|
||||
bestFraction = pairOutput.fraction;
|
||||
result = pairOutput;
|
||||
result.point = b3Add( result.point, origin );
|
||||
result.triangleIndex = triangleIndex2;
|
||||
result.materialIndex = materialIndex;
|
||||
// Shift origin to first vertex
|
||||
b3Vec3 origin = point21;
|
||||
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) };
|
||||
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
pairInput.maxFraction = bestFraction;
|
||||
pairInput.transform.p = b3Neg( origin );
|
||||
|
||||
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
|
||||
|
||||
if ( pairOutput.hit )
|
||||
{
|
||||
bestFraction = pairOutput.fraction;
|
||||
result = pairOutput;
|
||||
result.point = b3Add( result.point, origin );
|
||||
result.triangleIndex = triangleIndex2;
|
||||
result.materialIndex = materialIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1055,8 +1055,7 @@ bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTran
|
|||
|
||||
b3SimplexCache cache = { 0 };
|
||||
|
||||
// Outer loop on rows and inner loop on columns so that triangle indices
|
||||
// increase monotonically.
|
||||
// Outer loop on rows and inner loop on columns so that triangle indices increase monotonically.
|
||||
for ( int row = minRow; row <= maxRow; ++row )
|
||||
{
|
||||
if ( row < 0 || shape->rowCount - 1 <= row )
|
||||
|
|
@ -1210,6 +1209,7 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H
|
|||
b3SimplexCache cache = { 0 };
|
||||
|
||||
float radius = mover->radius;
|
||||
b3Vec3 center = b3Lerp( mover->center1, mover->center2, 0.5f );
|
||||
b3V32 center1 = b3LoadV( &mover->center1.x );
|
||||
b3V32 center2 = b3LoadV( &mover->center2.x );
|
||||
b3V32 r = b3SplatV( radius );
|
||||
|
|
@ -1229,6 +1229,8 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H
|
|||
int minCol = (int)floorf( localMinX / scale.x );
|
||||
int maxCol = (int)floorf( localMaxX / scale.x );
|
||||
|
||||
bool clockWise = shape->clockwise;
|
||||
|
||||
int planeCount = 0;
|
||||
|
||||
// Outer loop on rows and inner loop on columns so that triangle indices
|
||||
|
|
@ -1262,63 +1264,84 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H
|
|||
b3Vec3 point21 = corners[2];
|
||||
b3Vec3 point22 = corners[3];
|
||||
|
||||
if ( clockWise )
|
||||
{
|
||||
B3_SWAP( point12, point21 );
|
||||
}
|
||||
|
||||
b3V32 v11 = b3LoadV( &point11.x );
|
||||
b3V32 v12 = b3LoadV( &point12.x );
|
||||
b3V32 v21 = b3LoadV( &point21.x );
|
||||
b3V32 v22 = b3LoadV( &point22.x );
|
||||
|
||||
if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) )
|
||||
bool overlap1 = b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 );
|
||||
if ( overlap1 )
|
||||
{
|
||||
b3Vec3 triangleVertices[] = { point11, point21, point12 };
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
float signedVolume = b3SignedVolume( point11, point21, point12, center );
|
||||
|
||||
// reset the cache
|
||||
cache.count = 0;
|
||||
|
||||
// get distance between triangle and mover
|
||||
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );
|
||||
|
||||
if ( distanceOutput.distance == 0.0f )
|
||||
// Front side?
|
||||
if ( signedVolume >= 0.0f )
|
||||
{
|
||||
// todo SAT
|
||||
}
|
||||
else if ( distanceOutput.distance <= mover->radius )
|
||||
{
|
||||
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
|
||||
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA };
|
||||
planeCount += 1;
|
||||
b3Vec3 triangleVertices[] = { point11, point21, point12 };
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
|
||||
if ( planeCount == capacity )
|
||||
// reset the cache
|
||||
cache.count = 0;
|
||||
|
||||
// get distance between triangle and mover
|
||||
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );
|
||||
|
||||
if ( distanceOutput.distance == 0.0f )
|
||||
{
|
||||
return planeCount;
|
||||
// deep overlap
|
||||
}
|
||||
else if ( distanceOutput.distance <= mover->radius )
|
||||
{
|
||||
int triangleIndex = 2 * cellIndex;
|
||||
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
|
||||
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, material };
|
||||
planeCount += 1;
|
||||
|
||||
if ( planeCount == capacity )
|
||||
{
|
||||
return planeCount;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) )
|
||||
bool overlap2 = b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 );
|
||||
if ( overlap2 )
|
||||
{
|
||||
b3Vec3 triangleVertices[] = { point22, point12, point21 };
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
float signedVolume = b3SignedVolume( point22, point12, point21, center );
|
||||
|
||||
// reset the cache
|
||||
cache.count = 0;
|
||||
|
||||
// get distance between triangle and mover
|
||||
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );
|
||||
|
||||
if ( distanceOutput.distance == 0.0f )
|
||||
// Front side?
|
||||
if ( signedVolume >= 0.0f )
|
||||
{
|
||||
// todo SAT
|
||||
}
|
||||
else if ( distanceOutput.distance <= mover->radius )
|
||||
{
|
||||
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
|
||||
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA };
|
||||
planeCount += 1;
|
||||
b3Vec3 triangleVertices[] = { point22, point12, point21 };
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
|
||||
if ( planeCount == capacity )
|
||||
// reset the cache
|
||||
cache.count = 0;
|
||||
|
||||
// get distance between triangle and mover
|
||||
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );
|
||||
|
||||
if ( distanceOutput.distance == 0.0f )
|
||||
{
|
||||
return planeCount;
|
||||
// deep overlap
|
||||
}
|
||||
else if ( distanceOutput.distance <= mover->radius )
|
||||
{
|
||||
int triangleIndex = 2 * cellIndex + 1;
|
||||
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
|
||||
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, material };
|
||||
planeCount += 1;
|
||||
|
||||
if ( planeCount == capacity )
|
||||
{
|
||||
return planeCount;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2655,7 +2655,7 @@ int b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const
|
|||
if ( distanceOutput.distance <= totalRadius )
|
||||
{
|
||||
b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance };
|
||||
*result = (b3PlaneResult){ plane, distanceOutput.pointA };
|
||||
*result = (b3PlaneResult){ plane, distanceOutput.pointA, 0, 0, 0 };
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
|
|||
85
src/joint.c
85
src/joint.c
|
|
@ -120,7 +120,7 @@ b3ExplosionDef b3DefaultExplosionDef( void )
|
|||
b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId )
|
||||
{
|
||||
int id = jointId.index1 - 1;
|
||||
b3Joint* joint = b3Array_Get( world->joints, id );
|
||||
b3Joint* joint = b3Array_Get( world->joints, id );
|
||||
B3_ASSERT( joint->jointId == id && joint->generation == jointId.generation );
|
||||
return joint;
|
||||
}
|
||||
|
|
@ -131,11 +131,11 @@ b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint )
|
|||
{
|
||||
B3_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B3_GRAPH_COLOR_COUNT );
|
||||
b3GraphColor* color = world->constraintGraph.colors + joint->colorIndex;
|
||||
return b3Array_Get( color->jointSims, joint->localIndex );
|
||||
return b3Array_Get( color->jointSims, joint->localIndex );
|
||||
}
|
||||
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex );
|
||||
return b3Array_Get( set->jointSims, joint->localIndex );
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex );
|
||||
return b3Array_Get( set->jointSims, joint->localIndex );
|
||||
}
|
||||
|
||||
b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type )
|
||||
|
|
@ -171,10 +171,10 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
int jointId = b3AllocId( &world->jointIdPool );
|
||||
if ( jointId == world->joints.count )
|
||||
{
|
||||
b3Array_Push( world->joints, (b3Joint){ 0 } );
|
||||
b3Array_Push( world->joints, (b3Joint){ 0 } );
|
||||
}
|
||||
|
||||
b3Joint* joint = b3Array_Get( world->joints, jointId );
|
||||
b3Joint* joint = b3Array_Get( world->joints, jointId );
|
||||
joint->jointId = jointId;
|
||||
joint->userData = def->userData;
|
||||
joint->generation += 1;
|
||||
|
|
@ -195,7 +195,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
int keyA = ( jointId << 1 ) | 0;
|
||||
if ( bodyA->headJointKey != B3_NULL_INDEX )
|
||||
{
|
||||
b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 );
|
||||
b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 );
|
||||
b3JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 );
|
||||
edgeA->prevKey = keyA;
|
||||
}
|
||||
|
|
@ -210,7 +210,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
int keyB = ( jointId << 1 ) | 1;
|
||||
if ( bodyB->headJointKey != B3_NULL_INDEX )
|
||||
{
|
||||
b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 );
|
||||
b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 );
|
||||
b3JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 );
|
||||
edgeB->prevKey = keyB;
|
||||
}
|
||||
|
|
@ -222,7 +222,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet )
|
||||
{
|
||||
// if either body is disabled, create in disabled set
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet );
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet );
|
||||
joint->setIndex = b3_disabledSet;
|
||||
joint->localIndex = set->jointSims.count;
|
||||
|
||||
|
|
@ -236,7 +236,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
else if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody )
|
||||
{
|
||||
// joint is not attached to a dynamic body
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet );
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet );
|
||||
joint->setIndex = b3_staticSet;
|
||||
joint->localIndex = set->jointSims.count;
|
||||
|
||||
|
|
@ -271,7 +271,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
// joint should go into the sleeping set (not static set)
|
||||
int setIndex = maxSetIndex;
|
||||
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
|
||||
joint->setIndex = setIndex;
|
||||
joint->localIndex = set->jointSims.count;
|
||||
|
||||
|
|
@ -292,10 +292,10 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint
|
|||
// fix potentially invalid set index
|
||||
setIndex = bodyA->setIndex;
|
||||
|
||||
b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex );
|
||||
b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex );
|
||||
|
||||
// Careful! The joint sim pointer was orphaned by the set merge.
|
||||
jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex );
|
||||
jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex );
|
||||
}
|
||||
|
||||
B3_ASSERT( joint->setIndex == setIndex );
|
||||
|
|
@ -359,7 +359,7 @@ static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Bod
|
|||
int contactId = contactKey >> 1;
|
||||
int edgeIndex = contactKey & 1;
|
||||
|
||||
b3Contact* contact = b3Array_Get( world->contacts, contactId );
|
||||
b3Contact* contact = b3Array_Get( world->contacts, contactId );
|
||||
contactKey = contact->edges[edgeIndex].nextKey;
|
||||
|
||||
int otherEdgeIndex = edgeIndex ^ 1;
|
||||
|
|
@ -437,7 +437,7 @@ b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* de
|
|||
{
|
||||
B3_CHECK_JOINT_DEF( def );
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -505,7 +505,7 @@ b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def )
|
|||
{
|
||||
B3_CHECK_JOINT_DEF( def );
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -523,7 +523,7 @@ b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* de
|
|||
{
|
||||
B3_CHECK_JOINT_DEF( def );
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -552,7 +552,7 @@ b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef*
|
|||
B3_ASSERT( def->lowerTranslation <= def->upperTranslation );
|
||||
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -583,7 +583,7 @@ b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* de
|
|||
B3_CHECK_JOINT_DEF( def );
|
||||
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -620,7 +620,7 @@ b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef*
|
|||
B3_ASSERT( b3IsValidQuat( def->targetRotation ) );
|
||||
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -661,7 +661,7 @@ b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def )
|
|||
B3_ASSERT( 0.0f <= def->linearDampingRatio );
|
||||
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -687,7 +687,7 @@ b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def )
|
|||
B3_ASSERT( def->lowerSuspensionLimit <= def->upperSuspensionLimit );
|
||||
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if (world == NULL)
|
||||
if ( world == NULL )
|
||||
{
|
||||
return (b3JointId){ 0 };
|
||||
}
|
||||
|
|
@ -730,20 +730,20 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies )
|
|||
|
||||
int idA = edgeA->bodyId;
|
||||
int idB = edgeB->bodyId;
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, idA );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, idB );
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, idA );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, idB );
|
||||
|
||||
// Remove from body A
|
||||
if ( edgeA->prevKey != B3_NULL_INDEX )
|
||||
{
|
||||
b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 );
|
||||
b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 );
|
||||
b3JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 );
|
||||
prevEdge->nextKey = edgeA->nextKey;
|
||||
}
|
||||
|
||||
if ( edgeA->nextKey != B3_NULL_INDEX )
|
||||
{
|
||||
b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 );
|
||||
b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 );
|
||||
b3JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 );
|
||||
nextEdge->prevKey = edgeA->prevKey;
|
||||
}
|
||||
|
|
@ -759,14 +759,14 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies )
|
|||
// Remove from body B
|
||||
if ( edgeB->prevKey != B3_NULL_INDEX )
|
||||
{
|
||||
b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 );
|
||||
b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 );
|
||||
b3JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 );
|
||||
prevEdge->nextKey = edgeB->nextKey;
|
||||
}
|
||||
|
||||
if ( edgeB->nextKey != B3_NULL_INDEX )
|
||||
{
|
||||
b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 );
|
||||
b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 );
|
||||
b3JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 );
|
||||
nextEdge->prevKey = edgeB->prevKey;
|
||||
}
|
||||
|
|
@ -799,14 +799,14 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies )
|
|||
}
|
||||
else
|
||||
{
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
|
||||
int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex );
|
||||
b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
|
||||
int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex );
|
||||
if ( movedIndex != B3_NULL_INDEX )
|
||||
{
|
||||
// Fix moved joint
|
||||
b3JointSim* movedJointSim = set->jointSims.data + localIndex;
|
||||
int movedId = movedJointSim->jointId;
|
||||
b3Joint* movedJoint = b3Array_Get( world->joints, movedId );
|
||||
b3Joint* movedJoint = b3Array_Get( world->joints, movedId );
|
||||
B3_ASSERT( movedJoint->localIndex == movedIndex );
|
||||
movedJoint->localIndex = localIndex;
|
||||
}
|
||||
|
|
@ -926,8 +926,8 @@ void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide )
|
|||
|
||||
joint->collideConnected = shouldCollide;
|
||||
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
|
||||
|
||||
if ( shouldCollide )
|
||||
{
|
||||
|
|
@ -939,7 +939,7 @@ void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide )
|
|||
int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId;
|
||||
while ( shapeId != B3_NULL_INDEX )
|
||||
{
|
||||
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
|
||||
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
|
||||
|
||||
if ( shape->proxyKey != B3_NULL_INDEX )
|
||||
{
|
||||
|
|
@ -989,8 +989,8 @@ void b3Joint_WakeBodies( b3JointId jointId )
|
|||
world->locked = true;
|
||||
|
||||
b3Joint* joint = b3GetJointFullId( world, jointId );
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
|
||||
|
||||
b3WakeBody( world, bodyA );
|
||||
b3WakeBody( world, bodyB );
|
||||
|
|
@ -998,6 +998,15 @@ void b3Joint_WakeBodies( b3JointId jointId )
|
|||
world->locked = false;
|
||||
}
|
||||
|
||||
bool b3Joint_IsAwake( b3JointId jointId )
|
||||
{
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
b3Joint* joint = b3GetJointFullId( world, jointId );
|
||||
|
||||
// Cheaper than checking if the bodies are awake.
|
||||
return joint->setIndex == b3_awakeSet;
|
||||
}
|
||||
|
||||
void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque )
|
||||
{
|
||||
float linearImpulse = 0.0f;
|
||||
|
|
@ -1651,8 +1660,8 @@ void b3SolveJoints_Overflow( b3StepContext* context, bool useBias )
|
|||
|
||||
void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint )
|
||||
{
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
|
||||
if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet )
|
||||
{
|
||||
return;
|
||||
|
|
|
|||
235
src/mesh.c
235
src/mesh.c
|
|
@ -244,12 +244,12 @@ static bool b3IsConsistent( const b3MeshData* mesh )
|
|||
const b3MeshNode* child2 = b3GetRightChild( node );
|
||||
b3AABB bounds2 = b3GetNodeAABB( child2 );
|
||||
|
||||
if ( !b3AABB_Contains( nodeBounds, bounds1 ) )
|
||||
if ( b3AABB_Contains( nodeBounds, bounds1 ) == false )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if ( !b3AABB_Contains( nodeBounds, bounds2 ) )
|
||||
if ( b3AABB_Contains( nodeBounds, bounds2 ) == false )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
|
@ -329,6 +329,11 @@ bool b3IsValidMesh( const b3MeshData* meshData )
|
|||
|
||||
#endif
|
||||
|
||||
static inline b3Vec3 b3GetStridedVertex( const b3Vec3* vertices, int index, size_t stride )
|
||||
{
|
||||
return *(const b3Vec3*)( (const uint8_t*)vertices + index * stride );
|
||||
}
|
||||
|
||||
// Node for a vertex linked list
|
||||
typedef struct b3VertexNode
|
||||
{
|
||||
|
|
@ -350,15 +355,17 @@ typedef struct b3SpatialHash
|
|||
b3Array( b3VertexNode ) nodes;
|
||||
const b3Vec3* vertices;
|
||||
int vertexCount;
|
||||
size_t stride;
|
||||
b3VertexMap vertexMap;
|
||||
float cellSize;
|
||||
float tolerance;
|
||||
} b3SpatialHash;
|
||||
|
||||
static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, float tolerance )
|
||||
static void b3CreateSpatialHash( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, size_t stride, float tolerance )
|
||||
{
|
||||
h->vertices = vertices;
|
||||
h->vertexCount = vertexCount;
|
||||
h->stride = stride;
|
||||
h->tolerance = tolerance;
|
||||
h->cellSize = 2.0f * tolerance;
|
||||
b3Array_CreateN( h->nodes, vertexCount );
|
||||
|
|
@ -369,7 +376,7 @@ static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int
|
|||
B3_ASSERT( h->cellSize > 0.0f );
|
||||
}
|
||||
|
||||
static void b3SpatialHash_Destroy( b3SpatialHash* h )
|
||||
static void b3DestroySpatialHash( b3SpatialHash* h )
|
||||
{
|
||||
b3VertexMap_cleanup( &h->vertexMap );
|
||||
b3Array_Destroy( h->nodes );
|
||||
|
|
@ -380,7 +387,8 @@ static void b3SpatialHash_Destroy( b3SpatialHash* h )
|
|||
static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentIndex )
|
||||
{
|
||||
B3_ASSERT( currentIndex < h->vertexCount );
|
||||
b3Vec3 vertex = h->vertices[currentIndex];
|
||||
size_t stride = h->stride;
|
||||
b3Vec3 vertex = b3GetStridedVertex( h->vertices, currentIndex, stride );
|
||||
float cellSize = h->cellSize;
|
||||
float tolerance = h->tolerance;
|
||||
|
||||
|
|
@ -420,7 +428,7 @@ static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentInd
|
|||
B3_ASSERT( existingIndex < currentIndex );
|
||||
B3_ASSERT( existingIndex < h->vertexCount );
|
||||
|
||||
b3Vec3 other = h->vertices[existingIndex];
|
||||
b3Vec3 other = b3GetStridedVertex( h->vertices, existingIndex, stride );
|
||||
|
||||
// IsEqual inlined: check if vertices are within tolerance
|
||||
if ( fabsf( vertex.x - other.x ) <= tolerance && fabsf( vertex.y - other.y ) <= tolerance &&
|
||||
|
|
@ -481,16 +489,18 @@ typedef struct b3WeldData
|
|||
|
||||
int vertexCount;
|
||||
int indexCount;
|
||||
size_t stride;
|
||||
} b3WeldData;
|
||||
|
||||
static int b3WeldVertices( b3WeldData* data, float tolerance )
|
||||
{
|
||||
int vertexCount = data->vertexCount;
|
||||
int uniqueCount = 0;
|
||||
size_t stride = data->stride;
|
||||
|
||||
// Create spatial hash and find duplicates
|
||||
b3SpatialHash spatialHash;
|
||||
b3SpatialHash_Create( &spatialHash, data->srcVertices, vertexCount, tolerance );
|
||||
b3CreateSpatialHash( &spatialHash, data->srcVertices, vertexCount, stride, tolerance );
|
||||
b3Array( int ) vertexMapping = { 0 };
|
||||
b3Array_Resize( vertexMapping, vertexCount );
|
||||
|
||||
|
|
@ -502,7 +512,7 @@ static int b3WeldVertices( b3WeldData* data, float tolerance )
|
|||
{
|
||||
// New unique vertex
|
||||
vertexMapping.data[i] = uniqueCount;
|
||||
data->dstVertices[uniqueCount] = data->srcVertices[i];
|
||||
data->dstVertices[uniqueCount] = b3GetStridedVertex( data->srcVertices, i, stride );
|
||||
uniqueCount += 1;
|
||||
}
|
||||
else
|
||||
|
|
@ -517,11 +527,11 @@ static int b3WeldVertices( b3WeldData* data, float tolerance )
|
|||
for ( int i = 0; i < indexCount; ++i )
|
||||
{
|
||||
int srcIndex = data->srcIndices[i];
|
||||
B3_ASSERT( srcIndex < vertexCount );
|
||||
B3_ASSERT( 0 <= srcIndex && srcIndex < vertexCount );
|
||||
data->dstIndices[i] = vertexMapping.data[srcIndex];
|
||||
}
|
||||
|
||||
b3SpatialHash_Destroy( &spatialHash );
|
||||
b3DestroySpatialHash( &spatialHash );
|
||||
b3Array_Destroy( vertexMapping );
|
||||
|
||||
return uniqueCount;
|
||||
|
|
@ -1555,9 +1565,36 @@ b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, f
|
|||
return b3CreateMesh( &def, NULL, 0 );
|
||||
}
|
||||
|
||||
static void b3CopyVerticesWithStride( b3Vec3* dst, const b3Vec3* src, int count, size_t stride )
|
||||
{
|
||||
if ( stride == sizeof( b3Vec3 ) )
|
||||
{
|
||||
memcpy( dst, src, count * sizeof( b3Vec3 ) );
|
||||
return;
|
||||
}
|
||||
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
dst[i] = *(const b3Vec3*)( (const uint8_t*)src + i * stride );
|
||||
}
|
||||
}
|
||||
|
||||
// Stride larger than this likely indicates stride is uninitialized memory.
|
||||
#define B3_MAX_STRIDE 4096
|
||||
|
||||
// todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE
|
||||
b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity )
|
||||
{
|
||||
if ( def->stride != 0 && ( def->stride < sizeof( b3Vec3 ) || B3_MAX_STRIDE < def->stride ) )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( ( def->stride & 3 ) != 0 )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( def->vertexCount < 3 || def->vertices == NULL || def->triangleCount <= 0 || def->indices == NULL )
|
||||
{
|
||||
return NULL;
|
||||
|
|
@ -1570,6 +1607,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
}
|
||||
|
||||
int vertexCount = def->vertexCount;
|
||||
size_t stride = def->stride == 0 ? sizeof( b3Vec3 ) : def->stride;
|
||||
|
||||
b3AABB meshBounds = B3_BOUNDS3_EMPTY;
|
||||
|
||||
|
|
@ -1584,6 +1622,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
{
|
||||
b3Array_Resize( vertices, vertexCount );
|
||||
b3Array_Resize( indices, 3 * triangleCount );
|
||||
|
||||
b3WeldData data = {
|
||||
.srcVertices = def->vertices,
|
||||
.srcIndices = def->indices,
|
||||
|
|
@ -1591,14 +1630,17 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
.dstIndices = indices.data,
|
||||
.vertexCount = vertexCount,
|
||||
.indexCount = 3 * triangleCount,
|
||||
.stride = stride,
|
||||
};
|
||||
|
||||
vertices.count = b3WeldVertices( &data, def->weldTolerance );
|
||||
vertexCount = vertices.count;
|
||||
B3_ASSERT( vertexCount <= def->vertexCount );
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Array_Append( vertices, def->vertices, vertexCount );
|
||||
vertices.count = vertexCount;
|
||||
b3CopyVerticesWithStride( vertices.data, def->vertices, vertexCount, stride );
|
||||
b3Array_Append( indices, def->indices, 3 * triangleCount );
|
||||
}
|
||||
|
||||
|
|
@ -1608,6 +1650,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
float minArea = 0.01f * B3_LINEAR_SLOP * B3_LINEAR_SLOP;
|
||||
float surfaceArea = 0.0f;
|
||||
int materialCount = 1;
|
||||
bool clockWise = def->clockWiseWinding;
|
||||
|
||||
for ( int index = 0; index < triangleCount; ++index )
|
||||
{
|
||||
|
|
@ -1615,6 +1658,10 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
int index2 = indices.data[3 * index + 1];
|
||||
int index3 = indices.data[3 * index + 2];
|
||||
|
||||
B3_ASSERT( 0 <= index1 && index1 < vertexCount );
|
||||
B3_ASSERT( 0 <= index2 && index2 < vertexCount );
|
||||
B3_ASSERT( 0 <= index3 && index3 < vertexCount );
|
||||
|
||||
b3Vec3 vertex1 = vertices.data[index1];
|
||||
b3Vec3 vertex2 = vertices.data[index2];
|
||||
b3Vec3 vertex3 = vertices.data[index3];
|
||||
|
|
@ -1628,11 +1675,12 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
|
||||
if ( index1 != index2 && index1 != index3 && index2 != index3 )
|
||||
{
|
||||
degenerateCount += 1;
|
||||
if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity )
|
||||
{
|
||||
degenerateTriangleIndices[degenerateCount - 1] = index;
|
||||
degenerateTriangleIndices[degenerateCount] = index;
|
||||
}
|
||||
|
||||
degenerateCount += 1;
|
||||
}
|
||||
|
||||
continue;
|
||||
|
|
@ -1641,8 +1689,8 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
surfaceArea += area;
|
||||
|
||||
b3AABB box = {
|
||||
b3Min( vertex1, b3Min( vertex2, vertex3 ) ),
|
||||
b3Max( vertex1, b3Max( vertex2, vertex3 ) ),
|
||||
.lowerBound = b3Min( vertex1, b3Min( vertex2, vertex3 ) ),
|
||||
.upperBound = b3Max( vertex1, b3Max( vertex2, vertex3 ) ),
|
||||
};
|
||||
|
||||
b3Vec3 center = b3AABB_Center( box );
|
||||
|
|
@ -1668,6 +1716,8 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
if ( b3IsSaneAABB( meshBounds ) == false )
|
||||
{
|
||||
b3Array_Destroy( primitives );
|
||||
b3Array_Destroy( indices );
|
||||
b3Array_Destroy( vertices );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
|
@ -1724,9 +1774,18 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
for ( int index = 0; index < triangleCount; ++index )
|
||||
{
|
||||
b3Primitive primitive = primitives.data[index];
|
||||
triangles[index].index1 = indices.data[3 * primitive.triangleIndex + 0];
|
||||
triangles[index].index2 = indices.data[3 * primitive.triangleIndex + 1];
|
||||
triangles[index].index3 = indices.data[3 * primitive.triangleIndex + 2];
|
||||
|
||||
int i1 = 3 * primitive.triangleIndex + 0;
|
||||
int i2 = 3 * primitive.triangleIndex + 1;
|
||||
int i3 = 3 * primitive.triangleIndex + 2;
|
||||
if ( clockWise )
|
||||
{
|
||||
B3_SWAP( i2, i3 );
|
||||
}
|
||||
|
||||
triangles[index].index1 = indices.data[i1];
|
||||
triangles[index].index2 = indices.data[i2];
|
||||
triangles[index].index3 = indices.data[i3];
|
||||
flags[index] = 0;
|
||||
|
||||
// Copy material indices if they exist. Otherwise the material indices are all zeroes.
|
||||
|
|
@ -1745,6 +1804,9 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
{
|
||||
b3Array_Destroy( tempNodes );
|
||||
b3Array_Destroy( primitives );
|
||||
b3Array_Destroy( indices );
|
||||
b3Array_Destroy( vertices );
|
||||
b3Free( mesh, byteCount );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
|
@ -1762,7 +1824,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices,
|
|||
b3Array_Destroy( vertices );
|
||||
|
||||
mesh->hash = 0;
|
||||
mesh->hash = b3Hash64NonZero((uint8_t*)mesh, mesh->byteCount );
|
||||
mesh->hash = b3Hash64NonZero( (uint8_t*)mesh, mesh->byteCount );
|
||||
|
||||
return mesh;
|
||||
}
|
||||
|
|
@ -1896,7 +1958,7 @@ b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input )
|
|||
|
||||
b3V32 scale = b3LoadV( &meshScale.x );
|
||||
b3V32 invScale = b3DivV( b3_oneV, scale );
|
||||
bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f;
|
||||
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
|
||||
|
||||
// Use the inverse scaled ray for traversal of the BVH
|
||||
b3V32 invScaledRayStart = b3MulV( invScale, rayStart );
|
||||
|
|
@ -1936,16 +1998,16 @@ b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input )
|
|||
b3Vec3 vertex2, vertex3;
|
||||
|
||||
// The CPU should predict this branch
|
||||
if ( clockwise )
|
||||
{
|
||||
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
|
||||
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
|
||||
}
|
||||
else
|
||||
if ( ccw )
|
||||
{
|
||||
vertex2 = b3Mul( meshScale, vertices[triangle.index2] );
|
||||
vertex3 = b3Mul( meshScale, vertices[triangle.index3] );
|
||||
}
|
||||
else
|
||||
{
|
||||
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
|
||||
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
|
||||
}
|
||||
|
||||
// Collide ray with triangle in scaled space
|
||||
b3V32 v1 = b3LoadV( &vertex1.x );
|
||||
|
|
@ -2030,7 +2092,7 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input
|
|||
b3V32 scale = b3LoadV( &meshScale.x );
|
||||
b3V32 invScale = b3DivV( b3_oneV, scale );
|
||||
b3V32 absInvScale = b3AbsV( invScale );
|
||||
bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f;
|
||||
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
|
||||
|
||||
// Use the inverse scaled shape cast for traversal of the BVH
|
||||
b3V32 invScaledRayStart = b3MulV( invScale, rayStart );
|
||||
|
|
@ -2067,21 +2129,21 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input
|
|||
int triangleIndex = triangleOffset + index;
|
||||
b3MeshTriangle triangle = triangles[triangleIndex];
|
||||
|
||||
// Collide ray with triangle in scaled space
|
||||
// Collide shape with triangle in scaled space
|
||||
b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] );
|
||||
b3Vec3 vertex2, vertex3;
|
||||
|
||||
// The CPU should predict this branch
|
||||
if ( clockwise )
|
||||
{
|
||||
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
|
||||
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
|
||||
}
|
||||
else
|
||||
if ( ccw )
|
||||
{
|
||||
vertex2 = b3Mul( meshScale, vertices[triangle.index2] );
|
||||
vertex3 = b3Mul( meshScale, vertices[triangle.index3] );
|
||||
}
|
||||
else
|
||||
{
|
||||
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
|
||||
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
|
||||
}
|
||||
|
||||
b3V32 v1 = b3LoadV( &vertex1.x );
|
||||
b3V32 v2 = b3LoadV( &vertex2.x );
|
||||
|
|
@ -2090,43 +2152,54 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input
|
|||
b3V32 triangleMin = b3SubV( b3MinV( v1, b3MinV( v2, v3 ) ), shapeExtent );
|
||||
b3V32 triangleMax = b3AddV( b3MaxV( v1, b3MaxV( v2, v3 ) ), shapeExtent );
|
||||
|
||||
// Test triangle-ray overlap in scaled space
|
||||
if ( b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ) )
|
||||
// Test extended triangle vs ray bounds overlap in scaled space. Skip edge tests.
|
||||
bool overlap = b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax );
|
||||
if (overlap == false)
|
||||
{
|
||||
// Collide shape with triangle in scaled space
|
||||
b3Vec3 origin = vertex1;
|
||||
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) };
|
||||
b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity };
|
||||
continue;
|
||||
}
|
||||
|
||||
b3ShapeCastPairInput pairInput;
|
||||
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
pairInput.proxyB = input->proxy;
|
||||
pairInput.transform = shiftedOrigin;
|
||||
pairInput.maxFraction = bestOutput.fraction;
|
||||
pairInput.translationB = input->translation;
|
||||
pairInput.canEncroach = input->canEncroach;
|
||||
// This test is in scaled space. vertices and center are scaled.
|
||||
float signedVolume = b3SignedVolume( vertex1, vertex2, vertex3, center );
|
||||
if ( signedVolume < 0.0f )
|
||||
{
|
||||
// Backside
|
||||
continue;
|
||||
}
|
||||
|
||||
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
|
||||
// Collide shape with triangle in scaled space.
|
||||
b3Vec3 origin = vertex1;
|
||||
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) };
|
||||
b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity };
|
||||
|
||||
if ( pairOutput.hit )
|
||||
{
|
||||
pairOutput.point = b3Add( pairOutput.point, origin );
|
||||
b3ShapeCastPairInput pairInput;
|
||||
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
pairInput.proxyB = input->proxy;
|
||||
pairInput.transform = shiftedOrigin;
|
||||
pairInput.maxFraction = bestOutput.fraction;
|
||||
pairInput.translationB = input->translation;
|
||||
pairInput.canEncroach = input->canEncroach;
|
||||
|
||||
bestOutput = pairOutput;
|
||||
bestOutput.triangleIndex = triangleIndex;
|
||||
bestOutput.materialIndex = materialIndices[triangleIndex];
|
||||
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
|
||||
|
||||
// Update ray bounds in scaled space
|
||||
lambda = b3SplatV( pairOutput.fraction );
|
||||
rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) );
|
||||
rayMin = b3MinV( rayStart, rayEnd );
|
||||
rayMax = b3MaxV( rayStart, rayEnd );
|
||||
if ( pairOutput.hit )
|
||||
{
|
||||
pairOutput.point = b3Add( pairOutput.point, origin );
|
||||
|
||||
// Ray bounds in unscaled space
|
||||
invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) );
|
||||
invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd );
|
||||
invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd );
|
||||
}
|
||||
bestOutput = pairOutput;
|
||||
bestOutput.triangleIndex = triangleIndex;
|
||||
bestOutput.materialIndex = materialIndices[triangleIndex];
|
||||
|
||||
// Update ray bounds in scaled space
|
||||
lambda = b3SplatV( pairOutput.fraction );
|
||||
rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) );
|
||||
rayMin = b3MinV( rayStart, rayEnd );
|
||||
rayMax = b3MaxV( rayStart, rayEnd );
|
||||
|
||||
// Ray bounds in unscaled space
|
||||
invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) );
|
||||
invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd );
|
||||
invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2219,7 +2292,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
|
||||
b3SimplexCache cache = { 0 };
|
||||
float radius = mover->radius;
|
||||
|
||||
b3Vec3 center = b3Lerp( mover->center1, mover->center2, 0.5f );
|
||||
b3V32 center1 = b3LoadV( &mover->center1.x );
|
||||
b3V32 center2 = b3LoadV( &mover->center2.x );
|
||||
b3V32 r = b3SplatV( radius );
|
||||
|
|
@ -2228,6 +2301,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
|
||||
// Scale may have reflection so min/max may become invalid when unscaled
|
||||
b3Vec3 meshScale = shape->scale;
|
||||
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
|
||||
b3V32 scale = b3LoadV( &meshScale.x );
|
||||
b3V32 invScale = b3DivV( b3_oneV, scale );
|
||||
b3V32 temp1 = b3MulV( invScale, boundsMin );
|
||||
|
|
@ -2242,6 +2316,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
const b3MeshNode* node = b3GetRoot( shape->data );
|
||||
const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data );
|
||||
const b3Vec3* vertices = b3GetMeshVertices( shape->data );
|
||||
const uint8_t* materialIndices = b3GetMeshMaterialIndices( shape->data );
|
||||
|
||||
int planeCount = 0;
|
||||
while ( planeCount < capacity )
|
||||
|
|
@ -2264,6 +2339,12 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
b3Vec3 vertex1 = vertices[triangle.index1];
|
||||
b3Vec3 vertex2 = vertices[triangle.index2];
|
||||
b3Vec3 vertex3 = vertices[triangle.index3];
|
||||
|
||||
if ( ccw == false )
|
||||
{
|
||||
B3_SWAP( vertex2, vertex3 );
|
||||
}
|
||||
|
||||
b3V32 v1 = b3LoadV( &vertex1.x );
|
||||
b3V32 v2 = b3LoadV( &vertex2.x );
|
||||
b3V32 v3 = b3LoadV( &vertex3.x );
|
||||
|
|
@ -2271,10 +2352,19 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
// Test triangle bounds overlap in unscaled space
|
||||
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
|
||||
{
|
||||
// Compute shape distance in scaled space. Winding order doesn't matter.
|
||||
// todo implement one-sided collision?
|
||||
// Compute shape distance in scaled space.
|
||||
b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ),
|
||||
b3Mul( meshScale, vertex3 ) };
|
||||
|
||||
// Vertices and center are in scaled space.
|
||||
float signedVolume =
|
||||
b3SignedVolume( triangleVertices[0], triangleVertices[1], triangleVertices[2], center );
|
||||
if ( signedVolume < 0.0f )
|
||||
{
|
||||
// Backside
|
||||
continue;
|
||||
}
|
||||
|
||||
distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
|
||||
|
||||
// reset the cache
|
||||
|
|
@ -2285,12 +2375,13 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
|
||||
if ( distanceOutput.distance == 0.0f )
|
||||
{
|
||||
// todo SAT
|
||||
// deep overlap
|
||||
}
|
||||
else if ( distanceOutput.distance <= mover->radius )
|
||||
{
|
||||
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
|
||||
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA };
|
||||
planes[planeCount] =
|
||||
(b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, materialIndices[triangleIndex] };
|
||||
planeCount += 1;
|
||||
|
||||
if ( planeCount == capacity )
|
||||
|
|
@ -2325,7 +2416,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
|
|||
void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* context )
|
||||
{
|
||||
b3Vec3 meshScale = mesh->scale;
|
||||
bool clockwise = meshScale.x * meshScale.y * meshScale.z > 0.0f;
|
||||
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
|
||||
|
||||
// Scale may have reflection so min/max may become invalid when unscaled
|
||||
b3V32 scale = b3LoadV( &meshScale.x );
|
||||
|
|
@ -2370,13 +2461,13 @@ void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void*
|
|||
b3V32 v2 = b3LoadV( &vertex2.x );
|
||||
b3V32 v3 = b3LoadV( &vertex3.x );
|
||||
|
||||
// Perform triangle overlap test in unscaled space. Winding order doesn't matter.
|
||||
// Perform triangle overlap test in unscaled space.
|
||||
// todo it is possible that some margins are getting scaled
|
||||
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
|
||||
{
|
||||
b3Vec3 a = b3Mul( meshScale, vertex1 );
|
||||
b3Vec3 b, c;
|
||||
if ( clockwise )
|
||||
if ( ccw )
|
||||
{
|
||||
b = b3Mul( meshScale, vertex2 );
|
||||
c = b3Mul( meshScale, vertex3 );
|
||||
|
|
|
|||
|
|
@ -11,8 +11,6 @@
|
|||
|
||||
#include <stdio.h>
|
||||
|
||||
// This guards against excessive memory usage and complex collision
|
||||
#define B3_MAX_MESH_CONTACT_TRIANGLES 256
|
||||
#define B3_MAX_POINTS_PER_TRIANGLE 32
|
||||
|
||||
#if B3_ENABLE_VALIDATION
|
||||
|
|
|
|||
|
|
@ -609,9 +609,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
|
|||
continue;
|
||||
}
|
||||
|
||||
// Update contact respecting shape/body order (A,B). Bodies behind awake-set
|
||||
// contacts are always either awake or static - inline b3GetBodySim with that
|
||||
// invariant to skip the cross-TU call and per-call solverSets indirection.
|
||||
// Update contact respecting shape/body order (A,B)
|
||||
b3Body* bodyA = bodies + shapeA->bodyId;
|
||||
b3Body* bodyB = bodies + shapeB->bodyId;
|
||||
bool isStaticA = bodyA->type == b3_staticBody;
|
||||
|
|
@ -2811,7 +2809,6 @@ void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mov
|
|||
|
||||
b3Vec3 r = { mover->radius, mover->radius, mover->radius };
|
||||
|
||||
// Relative box lifted to world float with outward rounding, conservative for the tree
|
||||
b3AABB relBox;
|
||||
relBox.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r );
|
||||
relBox.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r );
|
||||
|
|
@ -2828,7 +2825,6 @@ void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mov
|
|||
|
||||
if ( world->recording != NULL )
|
||||
{
|
||||
// CollideMover returns void: no treestats tail, just the per-shape plane batches.
|
||||
b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );
|
||||
b3RecQueryCommit( world->recording, b3_recOpQueryCollideMover, &recWriter );
|
||||
}
|
||||
|
|
|
|||
|
|
@ -217,6 +217,9 @@ void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v )
|
|||
b3RecW_VEC3( buf, v.plane.normal );
|
||||
b3RecW_F32( buf, v.plane.offset );
|
||||
b3RecW_VEC3( buf, v.point );
|
||||
b3RecW_I32( buf, v.triangleIndex );
|
||||
b3RecW_I32( buf, v.childIndex );
|
||||
b3RecW_I32( buf, v.materialIndex );
|
||||
}
|
||||
|
||||
void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v )
|
||||
|
|
|
|||
|
|
@ -47,8 +47,8 @@ typedef struct b3World b3World;
|
|||
#define B3_REC_MAGIC 0x43523342u
|
||||
|
||||
// Major recording version is bumped when writers change.
|
||||
// Major version 4 added b3ShapeDef::enableSpeculativeContact
|
||||
#define B3_REC_VERSION_MAJOR 4
|
||||
// Major version 5 added b3PlaneResult fields.
|
||||
#define B3_REC_VERSION_MAJOR 5
|
||||
|
||||
// Minor tracks op-stream additions that keep the 48 byte header shape.
|
||||
// Minor version 4 added b3Shape_SetMeshMaterial, b3Shape_SetHull, b3Shape_SetMesh
|
||||
|
|
|
|||
|
|
@ -288,6 +288,9 @@ b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr )
|
|||
v.plane.normal = b3RecR_VEC3( rdr );
|
||||
v.plane.offset = b3RecR_F32( rdr );
|
||||
v.point = b3RecR_VEC3( rdr );
|
||||
v.triangleIndex = b3RecR_I32( rdr );
|
||||
v.childIndex = b3RecR_I32( rdr );
|
||||
v.materialIndex = b3RecR_I32( rdr );
|
||||
return v;
|
||||
}
|
||||
|
||||
|
|
@ -1801,7 +1804,8 @@ static bool b3RecReplayPlaneTrampoline( b3ShapeId id, const b3PlaneResult* plane
|
|||
const b3RecRecordedHit* h = &rc->hits[rc->cursor + i];
|
||||
if ( b3RecVec3Differs( h->plane.plane.normal, planes[i].plane.normal ) ||
|
||||
b3RecF32Differs( h->plane.plane.offset, planes[i].plane.offset ) ||
|
||||
b3RecVec3Differs( h->plane.point, planes[i].point ) )
|
||||
b3RecVec3Differs( h->plane.point, planes[i].point ) || h->plane.triangleIndex != planes[i].triangleIndex ||
|
||||
h->plane.childIndex != planes[i].childIndex || h->plane.materialIndex != planes[i].materialIndex )
|
||||
{
|
||||
rc->rdr->diverged = true;
|
||||
}
|
||||
|
|
|
|||
35
src/shape.c
35
src/shape.c
|
|
@ -778,7 +778,7 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter )
|
|||
b3Vec3 c1 = b3Sub( shape->capsule.center1, localCenter );
|
||||
b3Vec3 c2 = b3Sub( shape->capsule.center2, localCenter );
|
||||
b3Vec3 r = { radius, radius, radius };
|
||||
extent.maxExtent = b3Add( b3Max( c1, c2 ), r );
|
||||
extent.maxExtent = b3Add( b3Max( b3Abs( c1 ), b3Abs( c2 ) ), r );
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -798,9 +798,9 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter )
|
|||
{
|
||||
float radius = shape->sphere.radius;
|
||||
extent.minExtent = radius;
|
||||
b3Vec3 h = b3Abs( b3Sub( shape->sphere.center, localCenter ) );
|
||||
b3Vec3 r = { radius, radius, radius };
|
||||
b3Vec3 p = b3Add( b3Sub( shape->sphere.center, localCenter ), r );
|
||||
extent.maxExtent = b3Abs( b3Sub( p, localCenter ) );
|
||||
extent.maxExtent = b3Add( h, r );
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -816,7 +816,7 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter )
|
|||
float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) );
|
||||
extent.minExtent = b3MinFloat( r1, r2 );
|
||||
b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter );
|
||||
extent.maxExtent = b3Abs( p );
|
||||
extent.maxExtent = b3Abs( b3Sub( p, localCenter ) );
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -939,32 +939,6 @@ bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeP
|
|||
B3_ASSERT( false );
|
||||
return false;
|
||||
}
|
||||
|
||||
#if 0
|
||||
b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS];
|
||||
b3ShapeProxy localProxy;
|
||||
|
||||
b3Transform invTransform = b3InvertTransform( transform );
|
||||
b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q );
|
||||
|
||||
localProxy.count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS );
|
||||
for ( int i = 0; i < localProxy.count; ++i )
|
||||
{
|
||||
localPoints[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p );
|
||||
}
|
||||
|
||||
localProxy.points = localPoints;
|
||||
localProxy.radius = proxy->radius;
|
||||
|
||||
if ( type == b3_meshShape )
|
||||
{
|
||||
return b3OverlapMesh( &localProxy, shape->mesh.data, shape->mesh.scale );
|
||||
}
|
||||
|
||||
B3_ASSERT( type == b3_heightShape );
|
||||
|
||||
return b3OverlapHeightField( &localProxy, shape->heightField );
|
||||
#endif
|
||||
}
|
||||
|
||||
int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform,
|
||||
|
|
@ -1016,6 +990,7 @@ int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* sha
|
|||
{
|
||||
planes[i].plane.normal = b3RotateVector( transform.q, planes[i].plane.normal );
|
||||
planes[i].point = b3TransformPoint( transform, planes[i].point );
|
||||
planes[i].materialIndex = b3ClampInt( planes[i].materialIndex, 0, shape->materialCount - 1 );
|
||||
}
|
||||
|
||||
return planeCount;
|
||||
|
|
|
|||
|
|
@ -242,6 +242,6 @@ int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const
|
|||
}
|
||||
|
||||
b3Plane plane = { normal, totalRadius - distance };
|
||||
*result = ( b3PlaneResult ){ plane, shape->center };
|
||||
*result = (b3PlaneResult){ plane, shape->center, 0, 0, 0 };
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -478,6 +478,7 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi
|
|||
{
|
||||
b3Vec3 cdot = b3Sub( wB, wA );
|
||||
|
||||
// todo it might be more accurate to have the motor velocity in frameA due to sub-stepping.
|
||||
b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) );
|
||||
b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda );
|
||||
float length = b3Length( newImpulse );
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ set(BOX3D_TEST_FILES
|
|||
test_macros.h
|
||||
test_manifold.c
|
||||
test_math.c
|
||||
test_mesh.c
|
||||
test_mover.c
|
||||
test_name_cache.c
|
||||
test_recording.c
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@ extern int JointTest( void );
|
|||
extern int LargeWorldTest( void );
|
||||
extern int ManifoldTest( void );
|
||||
extern int MathTest( void );
|
||||
extern int MeshTest( void );
|
||||
extern int MoverTest( void );
|
||||
extern int NameCacheTest( void );
|
||||
extern int RecordingTest( void );
|
||||
|
|
@ -116,6 +117,7 @@ int main( int argc, char** argv )
|
|||
MAYBE_RUN_TEST( LargeWorldTest );
|
||||
MAYBE_RUN_TEST( ManifoldTest );
|
||||
MAYBE_RUN_TEST( MathTest );
|
||||
MAYBE_RUN_TEST( MeshTest );
|
||||
MAYBE_RUN_TEST( MoverTest );
|
||||
MAYBE_RUN_TEST( NameCacheTest );
|
||||
MAYBE_RUN_TEST( RecordingTest );
|
||||
|
|
|
|||
|
|
@ -385,6 +385,96 @@ static int SetMassDataConsistentVelocity( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Extents bound the shapes about the center of mass, per axis. An offset shape must count its
|
||||
// offset, not just its own size.
|
||||
static int ShapeExtents( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.density = 1.0f;
|
||||
|
||||
// Kinematic bodies measure from the body origin
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_kinematicBody;
|
||||
|
||||
b3BodyId capsuleId = b3CreateBody( worldId, &bodyDef );
|
||||
b3Capsule capsule = { { -2.0f, 0.0f, 0.0f }, { -1.0f, 0.0f, 0.0f }, 0.2f };
|
||||
b3CreateCapsuleShape( capsuleId, &shapeDef, &capsule );
|
||||
|
||||
b3Vec3 maxExtent = b3Body_GetMaxExtent( capsuleId );
|
||||
ENSURE_SMALL( maxExtent.x - 2.2f, 1e-5f );
|
||||
ENSURE_SMALL( maxExtent.y - 0.2f, 1e-5f );
|
||||
ENSURE_SMALL( maxExtent.z - 0.2f, 1e-5f );
|
||||
ENSURE_SMALL( b3Body_GetMinExtent( capsuleId ) - 0.2f, 1e-5f );
|
||||
|
||||
b3BodyId sphereId = b3CreateBody( worldId, &bodyDef );
|
||||
b3Sphere sphere = { { 1.0f, 2.0f, 3.0f }, 0.5f };
|
||||
b3CreateSphereShape( sphereId, &shapeDef, &sphere );
|
||||
|
||||
maxExtent = b3Body_GetMaxExtent( sphereId );
|
||||
ENSURE_SMALL( maxExtent.x - 1.5f, 1e-5f );
|
||||
ENSURE_SMALL( maxExtent.y - 2.5f, 1e-5f );
|
||||
ENSURE_SMALL( maxExtent.z - 3.5f, 1e-5f );
|
||||
ENSURE_SMALL( b3Body_GetMinExtent( sphereId ) - 0.5f, 1e-5f );
|
||||
|
||||
// Dynamic bodies measure from the center of mass. A light sphere hung off a cube pulls the
|
||||
// center toward it, so the far side of the sphere is the widest point.
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
b3BodyId cubeId = b3CreateBody( worldId, &bodyDef );
|
||||
b3BoxHull cube = b3MakeCubeHull( 0.5f );
|
||||
b3CreateHullShape( cubeId, &shapeDef, &cube.base );
|
||||
b3Sphere offsetSphere = { { 1.0f, 0.0f, 0.0f }, 0.2f };
|
||||
b3CreateSphereShape( cubeId, &shapeDef, &offsetSphere );
|
||||
|
||||
b3Vec3 localCenter = b3Body_GetLocalCenter( cubeId );
|
||||
ENSURE( 0.0f < localCenter.x && localCenter.x < 0.5f );
|
||||
|
||||
maxExtent = b3Body_GetMaxExtent( cubeId );
|
||||
ENSURE_SMALL( maxExtent.x - ( 1.2f - localCenter.x ), 1e-5f );
|
||||
ENSURE_SMALL( maxExtent.y - 0.5f, 1e-5f );
|
||||
ENSURE_SMALL( maxExtent.z - 0.5f, 1e-5f );
|
||||
ENSURE_SMALL( b3Body_GetMinExtent( cubeId ) - 0.2f, 1e-5f );
|
||||
|
||||
b3Vec3 originExtent = b3Body_GetMaxExtentOrigin( cubeId );
|
||||
ENSURE_SMALL( originExtent.x - 1.2f, 1e-5f );
|
||||
ENSURE_SMALL( originExtent.y - 0.5f, 1e-5f );
|
||||
ENSURE_SMALL( originExtent.z - 0.5f, 1e-5f );
|
||||
|
||||
// A mesh has no mass, so the sphere alone places the center at x = 1 and the far edge of the
|
||||
// mesh at x = -3 is four units out.
|
||||
b3Vec3 vertices[6] = {
|
||||
{ -3.0f, 0.0f, -1.0f }, { -2.0f, 0.0f, 1.0f }, { -1.0f, 0.0f, -1.0f },
|
||||
{ 1.0f, 0.0f, -1.0f }, { 2.0f, 0.0f, 1.0f }, { 3.0f, 0.0f, -1.0f },
|
||||
};
|
||||
int32_t indices[6] = { 0, 1, 2, 3, 4, 5 };
|
||||
b3MeshDef meshDef = { 0 };
|
||||
meshDef.vertices = vertices;
|
||||
meshDef.stride = sizeof( b3Vec3 );
|
||||
meshDef.indices = indices;
|
||||
meshDef.vertexCount = 6;
|
||||
meshDef.triangleCount = 2;
|
||||
b3MeshData* mesh = b3CreateMesh( &meshDef, NULL, 0 );
|
||||
|
||||
b3BodyId meshId = b3CreateBody( worldId, &bodyDef );
|
||||
b3CreateSphereShape( meshId, &shapeDef, &offsetSphere );
|
||||
b3CreateMeshShape( meshId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } );
|
||||
|
||||
localCenter = b3Body_GetLocalCenter( meshId );
|
||||
ENSURE_SMALL( localCenter.x - 1.0f, 1e-5f );
|
||||
|
||||
maxExtent = b3Body_GetMaxExtent( meshId );
|
||||
ENSURE_SMALL( maxExtent.x - 4.0f, 1e-4f );
|
||||
ENSURE_SMALL( maxExtent.y - 0.2f, 1e-4f );
|
||||
ENSURE_SMALL( maxExtent.z - 1.0f, 1e-4f );
|
||||
ENSURE_SMALL( b3Body_GetMinExtent( meshId ) - 0.2f, 1e-5f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BodyTest( void )
|
||||
{
|
||||
RUN_SUBTEST( FarSingleSphereMass );
|
||||
|
|
@ -395,5 +485,6 @@ int BodyTest( void )
|
|||
RUN_SUBTEST( SetMassDataFixedRotation );
|
||||
RUN_SUBTEST( SetMassDataZeroMass );
|
||||
RUN_SUBTEST( SetMassDataConsistentVelocity );
|
||||
RUN_SUBTEST( ShapeExtents );
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -565,6 +565,412 @@ static int MoverCapacity( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// TimeOfImpactMover ------------------------------------------------------------------------
|
||||
|
||||
// The mover capsule is expressed in the query frame, so a core segment starting at the origin
|
||||
// stands the character on the query point. Targets sit on the sweep line at y = 0.
|
||||
static b3Capsule MakeStandingMover( void )
|
||||
{
|
||||
return (b3Capsule){ { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, 0.25f };
|
||||
}
|
||||
|
||||
static int MoverTOIHitsBox( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3ShapeId shapeId = b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
// Face at x = 4.5, mover radius 0.25, so 4.25 of the 10 unit sweep is free.
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = IdentityAt( 5.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f },
|
||||
b3DefaultQueryFilter(), bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( result.fraction - 0.425f, 1e-2f );
|
||||
ENSURE( b3IsNormalized( result.normal ) );
|
||||
ENSURE_SMALL( result.normal.x + 1.0f, 1e-3f );
|
||||
|
||||
// The result carries a shape id, so the hit shape must come back identified.
|
||||
ENSURE( b3Shape_IsValid( result.shapeId ) );
|
||||
ENSURE( result.shapeId.index1 == shapeId.index1 );
|
||||
ENSURE( result.shapeId.generation == shapeId.generation );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverTOISeparated( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
// Sweeping along +Y holds the X gap at 4.25 for the whole interval.
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = IdentityAt( 5.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 0.0f, 10.0f, 0.0f },
|
||||
b3DefaultQueryFilter(), bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( result.fraction - 1.0f, 1e-6f );
|
||||
ENSURE( b3Shape_IsValid( result.shapeId ) == false );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverTOIOverlapped( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
// Mover starts buried in the box, so there is no free interval to search.
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f },
|
||||
b3DefaultQueryFilter(), bodyTransform, bodyTransform );
|
||||
|
||||
// Overlap should be ignored.
|
||||
ENSURE_SMALL( result.fraction - 1.0f, 1e-6f );
|
||||
ENSURE( B3_IS_NULL(result.shapeId) );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Two shapes on the sweep line must resolve to the nearer one whatever order the shape list
|
||||
// hands them to the loop. Shapes are pushed on the head of the list, so the two bodies below
|
||||
// walk their shapes in opposite orders.
|
||||
static int MoverTOIClosestShape( void )
|
||||
{
|
||||
b3BodyId nearFirstId;
|
||||
b3WorldId worldId = CreateQueryWorld( &nearFirstId );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
b3BodyId nearLastId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3Sphere nearSphere = { { 5.0f, 0.0f, 0.0f }, 0.5f };
|
||||
b3Sphere farSphere = { { 9.0f, 0.0f, 0.0f }, 0.5f };
|
||||
|
||||
b3ShapeId nearFirstHit = b3CreateSphereShape( nearFirstId, &shapeDef, &nearSphere );
|
||||
b3CreateSphereShape( nearFirstId, &shapeDef, &farSphere );
|
||||
|
||||
b3CreateSphereShape( nearLastId, &shapeDef, &farSphere );
|
||||
b3ShapeId nearLastHit = b3CreateSphereShape( nearLastId, &shapeDef, &nearSphere );
|
||||
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f );
|
||||
b3Vec3 translation = { 10.0f, 0.0f, 0.0f };
|
||||
|
||||
b3BodyTOIResult nearFirst = b3Body_TimeOfImpactMover( nearFirstId, b3Pos_zero, &mover, translation, b3DefaultQueryFilter(),
|
||||
bodyTransform, bodyTransform );
|
||||
b3BodyTOIResult nearLast = b3Body_TimeOfImpactMover( nearLastId, b3Pos_zero, &mover, translation, b3DefaultQueryFilter(),
|
||||
bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( nearFirst.fraction - 0.425f, 1e-2f );
|
||||
ENSURE( nearFirst.shapeId.index1 == nearFirstHit.index1 );
|
||||
|
||||
ENSURE_SMALL( nearLast.fraction - nearFirst.fraction, 1e-4f );
|
||||
ENSURE( nearLast.shapeId.index1 == nearLastHit.index1 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A shape the sweep clears must not erase a hit found earlier in the list. The off path sphere
|
||||
// is created first so the list hands it over last.
|
||||
static int MoverTOIKeepsHitAfterMiss( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3Sphere offPath = { { 5.0f, 0.0f, 20.0f }, 0.5f };
|
||||
b3Sphere onPath = { { 5.0f, 0.0f, 0.0f }, 0.5f };
|
||||
b3CreateSphereShape( bodyId, &shapeDef, &offPath );
|
||||
b3ShapeId onPathId = b3CreateSphereShape( bodyId, &shapeDef, &onPath );
|
||||
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f },
|
||||
b3DefaultQueryFilter(), bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( result.fraction - 0.425f, 1e-2f );
|
||||
ENSURE( result.shapeId.index1 == onPathId.index1 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The mover capsule points live in the query frame, so sliding both the mover and the body by
|
||||
// the same offset must not move the impact.
|
||||
static int MoverTOIMoverOffset( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
b3Vec3 translation = { 10.0f, 0.0f, 0.0f };
|
||||
b3QueryFilter filter = b3DefaultQueryFilter();
|
||||
|
||||
b3Capsule atOrigin = MakeStandingMover();
|
||||
b3WorldTransform transformA = IdentityAt( 5.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult resultA =
|
||||
b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &atOrigin, translation, filter, transformA, transformA );
|
||||
|
||||
b3Capsule offset = { { 2.0f, 0.0f, 0.0f }, { 2.0f, 1.0f, 0.0f }, 0.25f };
|
||||
b3WorldTransform transformB = IdentityAt( 7.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult resultB = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &offset, translation, filter, transformB, transformB );
|
||||
|
||||
ENSURE_SMALL( resultB.fraction - resultA.fraction, 1e-4f );
|
||||
ENSURE_SMALL( resultB.point.x - resultA.point.x - 2.0f, 1e-3f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The body sweeps between the two supplied transforms. A bar spinning a quarter turn about Y
|
||||
// reaches a mover that its start pose clears.
|
||||
static int MoverTOIRotatingBody( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull bar = b3MakeBoxHull( 2.0f, 0.25f, 0.25f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, &bar.base );
|
||||
|
||||
// Query origin puts the mover at world (0,0,2.1), just inside the swept end of the bar.
|
||||
b3Pos origin = { 0.0f, 0.0f, 2.1f };
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform transform1 = IdentityAt( 0.0f, 0.0f, 0.0f );
|
||||
b3WorldTransform transform2 = {
|
||||
.p = (b3Pos){ 0.0f, 0.0f, 0.0f },
|
||||
.q = b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.5f * B3_PI ),
|
||||
};
|
||||
|
||||
b3BodyTOIResult spinning =
|
||||
b3Body_TimeOfImpactMover( bodyId, origin, &mover, b3Vec3_zero, b3DefaultQueryFilter(), transform1, transform2 );
|
||||
|
||||
ENSURE( 0.0f < spinning.fraction && spinning.fraction < 1.0f );
|
||||
|
||||
// Holding the start pose leaves the bar along X and well clear.
|
||||
b3BodyTOIResult still =
|
||||
b3Body_TimeOfImpactMover( bodyId, origin, &mover, b3Vec3_zero, b3DefaultQueryFilter(), transform1, transform1 );
|
||||
|
||||
ENSURE_SMALL( still.fraction - 1.0f, 1e-6f );
|
||||
ENSURE( B3_IS_NULL( still.shapeId ) );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Everything is re-centered on the origin, so a huge origin must not shift the fraction and the
|
||||
// hit point comes back in the origin frame.
|
||||
static int MoverTOIFarFromOrigin( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
b3Pos origin = { 1.0e6f, -2.0e6f, 5.0e5f };
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = { .p = b3OffsetPos( origin, (b3Vec3){ 5.0f, 0.0f, 0.0f } ), .q = b3Quat_identity };
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, origin, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f },
|
||||
b3DefaultQueryFilter(), bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( result.fraction - 0.425f, 1e-2f );
|
||||
ENSURE_SMALL( result.normal.x + 1.0f, 1e-3f );
|
||||
ENSURE_SMALL( (result.point.x - origin.x) - 4.4f, 0.5f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverTOIFilter( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
// Geometry is on the sweep line, but a zero mask rejects every category.
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3QueryFilter filter = b3DefaultQueryFilter();
|
||||
filter.maskBits = 0;
|
||||
b3WorldTransform bodyTransform = IdentityAt( 5.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, filter,
|
||||
bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( result.fraction - 1.0f, 1e-6f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A shape the mover starts inside is skipped, but the sweep still runs against the other shapes
|
||||
// on the body.
|
||||
static int MoverTOIOverlapSkipsShape( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
||||
b3BoxHull buried = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3CreateHullShape( bodyId, &shapeDef, &buried.base );
|
||||
|
||||
// Face at x = 4.5 like MoverTOIHitsBox
|
||||
b3BoxHull onPath = b3MakeOffsetBoxHull( 0.5f, 0.5f, 0.5f, (b3Vec3){ 5.0f, 0.0f, 0.0f } );
|
||||
b3ShapeId onPathId = b3CreateHullShape( bodyId, &shapeDef, &onPath.base );
|
||||
|
||||
b3Capsule mover = MakeStandingMover();
|
||||
b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f );
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f },
|
||||
b3DefaultQueryFilter(), bodyTransform, bodyTransform );
|
||||
|
||||
ENSURE_SMALL( result.fraction - 0.425f, 1e-2f );
|
||||
ENSURE( result.shapeId.index1 == onPathId.index1 );
|
||||
ENSURE_SMALL( result.normal.x + 1.0f, 1e-3f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static float RandomFloat( uint32_t* seed, float lower, float upper )
|
||||
{
|
||||
uint32_t x = *seed;
|
||||
x ^= x << 13;
|
||||
x ^= x >> 17;
|
||||
x ^= x << 5;
|
||||
*seed = x;
|
||||
float unit = (float)( x >> 8 ) / 16777216.0f;
|
||||
return lower + ( upper - lower ) * unit;
|
||||
}
|
||||
|
||||
static b3Vec3 RandomVector( uint32_t* seed, float lower, float upper )
|
||||
{
|
||||
b3Vec3 v;
|
||||
v.x = RandomFloat( seed, lower, upper );
|
||||
v.y = RandomFloat( seed, lower, upper );
|
||||
v.z = RandomFloat( seed, lower, upper );
|
||||
return v;
|
||||
}
|
||||
|
||||
static b3Vec3 RandomDirection( uint32_t* seed )
|
||||
{
|
||||
for ( ;; )
|
||||
{
|
||||
b3Vec3 v = RandomVector( seed, -1.0f, 1.0f );
|
||||
float length = b3Length( v );
|
||||
if ( 0.1f < length && length < 1.0f )
|
||||
{
|
||||
return b3MulSV( 1.0f / length, v );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The mover sweep follows the solver's continuous rule: any time of impact strictly inside the
|
||||
// sweep counts, whatever the root finder reports. A capsule pivoting around a hull vertex can
|
||||
// exhaust the iteration cap, and that must not read as a clean miss. Compare against the raw
|
||||
// sweep built from the same inputs.
|
||||
static int MoverTOIMatchesSweep( void )
|
||||
{
|
||||
b3BodyId bodyId;
|
||||
b3WorldId worldId = CreateQueryWorld( &bodyId );
|
||||
|
||||
b3BoxHull bar = b3MakeBoxHull( 2.0f, 0.25f, 0.25f );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3ShapeId shapeId = b3CreateHullShape( bodyId, &shapeDef, &bar.base );
|
||||
|
||||
uint32_t seed = 0x9E3779B9u;
|
||||
int hitCount = 0;
|
||||
for ( int i = 0; i < 2000; ++i )
|
||||
{
|
||||
b3Capsule mover;
|
||||
mover.center1 = RandomVector( &seed, -0.2f, 0.2f );
|
||||
mover.center2 = b3MulAdd( mover.center1, RandomFloat( &seed, 0.05f, 1.5f ), RandomDirection( &seed ) );
|
||||
mover.radius = RandomFloat( &seed, 0.05f, 0.5f );
|
||||
b3Vec3 capsulePoints[2] = { mover.center1, mover.center2 };
|
||||
|
||||
b3Vec3 o = RandomVector( &seed, -3.5f, 3.5f );
|
||||
b3Pos origin = { o.x, o.y, o.z };
|
||||
b3Vec3 translation = b3MulSV( RandomFloat( &seed, 0.0f, 3.0f ), RandomDirection( &seed ) );
|
||||
|
||||
b3WorldTransform transform1 = {
|
||||
.p = b3Pos_zero,
|
||||
.q = b3MakeQuatFromAxisAngle( RandomDirection( &seed ), RandomFloat( &seed, 0.0f, B3_PI ) ),
|
||||
};
|
||||
b3Quat spin = b3MakeQuatFromAxisAngle( RandomDirection( &seed ), RandomFloat( &seed, 0.001f, B3_PI ) );
|
||||
b3Vec3 shift = RandomVector( &seed, -1.0f, 1.0f );
|
||||
b3WorldTransform transform2 = {
|
||||
.p = (b3Pos){ shift.x, shift.y, shift.z },
|
||||
.q = b3MulQuat( spin, transform1.q ),
|
||||
};
|
||||
|
||||
b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, origin, &mover, translation, b3DefaultQueryFilter(),
|
||||
transform1, transform2 );
|
||||
|
||||
// Same sweep the body query builds. The static body keeps its center at the body origin.
|
||||
b3Transform xf1 = b3ToRelativeTransform( transform1, origin );
|
||||
b3Transform xf2 = b3ToRelativeTransform( transform2, origin );
|
||||
|
||||
b3TOIInput input = { 0 };
|
||||
input.proxyA = (b3ShapeProxy){ b3GetHullPoints( &bar.base ), bar.base.vertexCount, 0.0f };
|
||||
input.proxyB = (b3ShapeProxy){ capsulePoints, 2, mover.radius };
|
||||
input.sweepA.c1 = xf1.p;
|
||||
input.sweepA.c2 = xf2.p;
|
||||
input.sweepA.q1 = transform1.q;
|
||||
input.sweepA.q2 = transform2.q;
|
||||
input.sweepA.localCenter = b3Vec3_zero;
|
||||
input.sweepB.c1 = b3Vec3_zero;
|
||||
input.sweepB.c2 = translation;
|
||||
input.sweepB.q1 = b3Quat_identity;
|
||||
input.sweepB.q2 = b3Quat_identity;
|
||||
input.sweepB.localCenter = b3Vec3_zero;
|
||||
input.maxFraction = 1.0f;
|
||||
|
||||
b3TOIOutput raw = b3TimeOfImpact( &input );
|
||||
|
||||
if ( 0.0f < raw.fraction && raw.fraction < 1.0f )
|
||||
{
|
||||
ENSURE_SMALL( result.fraction - raw.fraction, 1e-6f );
|
||||
ENSURE( result.shapeId.index1 == shapeId.index1 );
|
||||
ENSURE( b3IsNormalized( result.normal ) );
|
||||
ENSURE_SMALL( b3Distance( b3SubPos( result.point, origin ), raw.point ), 1e-4f );
|
||||
hitCount += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
ENSURE_SMALL( result.fraction - 1.0f, 1e-6f );
|
||||
ENSURE( B3_IS_NULL( result.shapeId ) );
|
||||
}
|
||||
}
|
||||
|
||||
ENSURE( hitCount > 0 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BodyQueryTest( void )
|
||||
{
|
||||
RUN_SUBTEST( CastRayHitsSphere );
|
||||
|
|
@ -589,6 +995,17 @@ int BodyQueryTest( void )
|
|||
RUN_SUBTEST( MoverSeparated );
|
||||
RUN_SUBTEST( MoverRotatedBody );
|
||||
RUN_SUBTEST( MoverCapacity );
|
||||
RUN_SUBTEST( MoverTOIHitsBox );
|
||||
RUN_SUBTEST( MoverTOISeparated );
|
||||
RUN_SUBTEST( MoverTOIOverlapped );
|
||||
RUN_SUBTEST( MoverTOIClosestShape );
|
||||
RUN_SUBTEST( MoverTOIKeepsHitAfterMiss );
|
||||
RUN_SUBTEST( MoverTOIMoverOffset );
|
||||
RUN_SUBTEST( MoverTOIRotatingBody );
|
||||
RUN_SUBTEST( MoverTOIFarFromOrigin );
|
||||
RUN_SUBTEST( MoverTOIFilter );
|
||||
RUN_SUBTEST( MoverTOIOverlapSkipsShape );
|
||||
RUN_SUBTEST( MoverTOIMatchesSweep );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -526,6 +526,153 @@ static int RayCastBruteForce( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Flat 3x3 vertex field at y = 0, one cell material per entry. Caller destroys.
|
||||
static b3HeightFieldData* MakeFlatField( uint8_t* materials, bool clockwise )
|
||||
{
|
||||
float heights[9] = { 0 };
|
||||
|
||||
b3HeightFieldDef def = { 0 };
|
||||
def.heights = heights;
|
||||
def.materialIndices = materials;
|
||||
def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f };
|
||||
def.countX = 3;
|
||||
def.countZ = 3;
|
||||
def.globalMinimumHeight = -1.0f;
|
||||
def.globalMaximumHeight = 1.0f;
|
||||
def.clockwiseWinding = clockwise;
|
||||
|
||||
return b3CreateHeightField( &def );
|
||||
}
|
||||
|
||||
static int ShapeCastBackside( void )
|
||||
{
|
||||
uint8_t materials[4] = { 1, 0, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, false );
|
||||
|
||||
// Falling onto the front (upper) face reports the hit, the triangle, and its material.
|
||||
// The surface sits at y = 0, so a sphere of radius 0.2 stops with its center at y = 0.2.
|
||||
b3Vec3 above = { 0.3f, 5.0f, 0.25f };
|
||||
b3ShapeCastInput down = { 0 };
|
||||
down.proxy = (b3ShapeProxy){ &above, 1, 0.2f };
|
||||
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
||||
down.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput out = b3ShapeCastHeightField( hf, &down );
|
||||
ENSURE( out.hit == true );
|
||||
ENSURE_SMALL( out.fraction - 0.48f, 1e-2f );
|
||||
ENSURE( out.normal.y > 0.99f );
|
||||
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
||||
ENSURE( out.materialIndex == 1 );
|
||||
|
||||
// Rising from behind the face is culled
|
||||
b3Vec3 below = { 0.3f, -5.0f, 0.25f };
|
||||
b3ShapeCastInput up = { 0 };
|
||||
up.proxy = (b3ShapeProxy){ &below, 1, 0.2f };
|
||||
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
||||
up.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput missed = b3ShapeCastHeightField( hf, &up );
|
||||
ENSURE( missed.hit == false );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A clockwise height field faces down, see HeightFieldWinding. Shape cast backside
|
||||
// culling must respect the flag: a rising sphere hits from below, not from above.
|
||||
static int ShapeCastClockwiseWinding( void )
|
||||
{
|
||||
uint8_t materials[4] = { 1, 0, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, true );
|
||||
|
||||
b3Vec3 below = { 0.3f, -5.0f, 0.25f };
|
||||
b3ShapeCastInput up = { 0 };
|
||||
up.proxy = (b3ShapeProxy){ &below, 1, 0.2f };
|
||||
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
||||
up.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput out = b3ShapeCastHeightField( hf, &up );
|
||||
ENSURE( out.hit == true );
|
||||
ENSURE_SMALL( out.fraction - 0.48f, 1e-2f );
|
||||
ENSURE( out.normal.y < -0.99f );
|
||||
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
||||
ENSURE( out.materialIndex == 1 );
|
||||
|
||||
b3Vec3 above = { 0.3f, 5.0f, 0.25f };
|
||||
b3ShapeCastInput down = { 0 };
|
||||
down.proxy = (b3ShapeProxy){ &above, 1, 0.2f };
|
||||
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
||||
down.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput missed = b3ShapeCastHeightField( hf, &down );
|
||||
ENSURE( missed.hit == false );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Ray casts run through the shape cast. The default winding faces up, so a ray from below is the
|
||||
// back side and gets culled.
|
||||
static int RayCastBackside( void )
|
||||
{
|
||||
uint8_t materials[4] = { 1, 0, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, false );
|
||||
|
||||
b3RayCastInput down = { 0 };
|
||||
down.origin = (b3Vec3){ 0.3f, 5.0f, 0.25f };
|
||||
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
||||
down.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput out = b3RayCastHeightField( hf, &down );
|
||||
ENSURE( out.hit == true );
|
||||
ENSURE_SMALL( out.fraction - 0.5f, 1e-3f );
|
||||
ENSURE( out.normal.y > 0.99f );
|
||||
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
||||
ENSURE( out.materialIndex == 1 );
|
||||
|
||||
b3RayCastInput up = { 0 };
|
||||
up.origin = (b3Vec3){ 0.3f, -5.0f, 0.25f };
|
||||
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
||||
up.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput missed = b3RayCastHeightField( hf, &up );
|
||||
ENSURE( missed.hit == false );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A clockwise field faces down. The winding swap already flips the triangle, so the ray normal
|
||||
// must not be flipped a second time.
|
||||
static int RayCastClockwiseWinding( void )
|
||||
{
|
||||
uint8_t materials[4] = { 1, 0, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, true );
|
||||
|
||||
b3RayCastInput up = { 0 };
|
||||
up.origin = (b3Vec3){ 0.3f, -5.0f, 0.25f };
|
||||
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
||||
up.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput out = b3RayCastHeightField( hf, &up );
|
||||
ENSURE( out.hit == true );
|
||||
ENSURE_SMALL( out.fraction - 0.5f, 1e-3f );
|
||||
ENSURE( out.normal.y < -0.99f );
|
||||
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
||||
ENSURE( out.materialIndex == 1 );
|
||||
|
||||
b3RayCastInput down = { 0 };
|
||||
down.origin = (b3Vec3){ 0.3f, 5.0f, 0.25f };
|
||||
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
||||
down.maxFraction = 1.0f;
|
||||
|
||||
b3CastOutput missed = b3RayCastHeightField( hf, &down );
|
||||
ENSURE( missed.hit == false );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
int HeightFieldTest( void )
|
||||
{
|
||||
RUN_SUBTEST( HeightFieldCreate );
|
||||
|
|
@ -537,6 +684,10 @@ int HeightFieldTest( void )
|
|||
RUN_SUBTEST( ShapeCastVerticalStraddle );
|
||||
RUN_SUBTEST( ShapeCastBruteForce );
|
||||
RUN_SUBTEST( RayCastBruteForce );
|
||||
RUN_SUBTEST( ShapeCastBackside );
|
||||
RUN_SUBTEST( ShapeCastClockwiseWinding );
|
||||
RUN_SUBTEST( RayCastBackside );
|
||||
RUN_SUBTEST( RayCastClockwiseWinding );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
355
test/test_mesh.c
Normal file
355
test/test_mesh.c
Normal file
|
|
@ -0,0 +1,355 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "test_macros.h"
|
||||
|
||||
#include "box3d/collision.h"
|
||||
#include "box3d/math_functions.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <string.h>
|
||||
|
||||
// Two quads meeting at a concave crease along the shared edge 1-4. The crease exercises
|
||||
// edge identification, which reads the baked winding rather than the input winding.
|
||||
#define VALLEY_VERTEX_COUNT 6
|
||||
#define VALLEY_TRIANGLE_COUNT 4
|
||||
|
||||
static void MakeValley( b3Vec3* vertices, int32_t* indices )
|
||||
{
|
||||
b3Vec3 v[VALLEY_VERTEX_COUNT] = {
|
||||
{ -1.0f, 1.0f, -1.0f }, { 0.0f, 0.0f, -1.0f }, { 1.0f, 1.0f, -1.0f },
|
||||
{ -1.0f, 1.0f, 1.0f }, { 0.0f, 0.0f, 1.0f }, { 1.0f, 1.0f, 1.0f },
|
||||
};
|
||||
|
||||
int32_t i[3 * VALLEY_TRIANGLE_COUNT] = {
|
||||
0, 3, 1, // left quad
|
||||
3, 4, 1,
|
||||
1, 4, 2, // right quad
|
||||
4, 5, 2,
|
||||
};
|
||||
|
||||
memcpy( vertices, v, sizeof( v ) );
|
||||
memcpy( indices, i, sizeof( i ) );
|
||||
}
|
||||
|
||||
// A vertex laid out the way a renderer would hand it over. The position sits off the
|
||||
// front of the struct so a non-zero base offset gets exercised too.
|
||||
typedef struct FatVertex
|
||||
{
|
||||
float weight;
|
||||
b3Vec3 position;
|
||||
float uv[2];
|
||||
} FatVertex;
|
||||
|
||||
static void MakeFatVertices( FatVertex* fat, const b3Vec3* vertices, int count )
|
||||
{
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
// Poison the padding so a stride mistake bakes obvious garbage instead of near misses
|
||||
fat[i].weight = 1000.0f + (float)i;
|
||||
fat[i].position = vertices[i];
|
||||
fat[i].uv[0] = -1000.0f;
|
||||
fat[i].uv[1] = -2000.0f;
|
||||
}
|
||||
}
|
||||
|
||||
static void ReverseWinding( int32_t* indices, int triangleCount )
|
||||
{
|
||||
for ( int i = 0; i < triangleCount; ++i )
|
||||
{
|
||||
int32_t temp = indices[3 * i + 1];
|
||||
indices[3 * i + 1] = indices[3 * i + 2];
|
||||
indices[3 * i + 2] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// The hash covers every byte of the mesh block, so this compares the tree, the vertices,
|
||||
// the baked triangles and the edge flags in one shot.
|
||||
static bool MeshesMatch( const b3MeshData* mesh1, const b3MeshData* mesh2 )
|
||||
{
|
||||
return mesh1->byteCount == mesh2->byteCount && mesh1->hash == mesh2->hash;
|
||||
}
|
||||
|
||||
static bool VerticesMatch( const b3MeshData* mesh, const b3Vec3* expected, int count )
|
||||
{
|
||||
if ( mesh->vertexCount != count )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const b3Vec3* vertices = b3GetMeshVertices( mesh );
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
if ( b3DistanceSquared( vertices[i], expected[i] ) > FLT_EPSILON )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// The valley faces up, so every baked triangle should wind counter clockwise about +Y
|
||||
static bool FacesUp( const b3MeshData* mesh )
|
||||
{
|
||||
const b3Vec3* vertices = b3GetMeshVertices( mesh );
|
||||
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh );
|
||||
|
||||
for ( int i = 0; i < mesh->triangleCount; ++i )
|
||||
{
|
||||
b3Vec3 v1 = vertices[triangles[i].index1];
|
||||
b3Vec3 v2 = vertices[triangles[i].index2];
|
||||
b3Vec3 v3 = vertices[triangles[i].index3];
|
||||
|
||||
b3Vec3 normal = b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) );
|
||||
if ( normal.y <= 0.0f )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool HasConcaveEdge( const b3MeshData* mesh )
|
||||
{
|
||||
const uint8_t* flags = b3GetMeshFlags( mesh );
|
||||
|
||||
for ( int i = 0; i < mesh->triangleCount; ++i )
|
||||
{
|
||||
if ( ( flags[i] & b3_allConcaveEdges ) != 0 )
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static b3MeshDef MakeValleyDef( b3Vec3* vertices, int32_t* indices )
|
||||
{
|
||||
b3MeshDef def = { 0 };
|
||||
def.vertices = vertices;
|
||||
def.stride = sizeof( b3Vec3 );
|
||||
def.indices = indices;
|
||||
def.vertexCount = VALLEY_VERTEX_COUNT;
|
||||
def.triangleCount = VALLEY_TRIANGLE_COUNT;
|
||||
def.identifyEdges = true;
|
||||
return def;
|
||||
}
|
||||
|
||||
// Dense input is the reference the other cases are measured against
|
||||
static int MeshDenseStride( void )
|
||||
{
|
||||
b3Vec3 vertices[VALLEY_VERTEX_COUNT];
|
||||
int32_t indices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
MakeValley( vertices, indices );
|
||||
|
||||
b3MeshDef def = MakeValleyDef( vertices, indices );
|
||||
b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 );
|
||||
ENSURE( mesh != NULL );
|
||||
|
||||
ENSURE( mesh->vertexCount == VALLEY_VERTEX_COUNT );
|
||||
ENSURE( mesh->triangleCount == VALLEY_TRIANGLE_COUNT );
|
||||
ENSURE( mesh->degenerateCount == 0 );
|
||||
ENSURE( VerticesMatch( mesh, vertices, VALLEY_VERTEX_COUNT ) );
|
||||
ENSURE( FacesUp( mesh ) );
|
||||
ENSURE( HasConcaveEdge( mesh ) );
|
||||
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Interleaved input must bake to exactly the same mesh as dense input
|
||||
static int MeshFatStride( void )
|
||||
{
|
||||
b3Vec3 vertices[VALLEY_VERTEX_COUNT];
|
||||
int32_t indices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
MakeValley( vertices, indices );
|
||||
|
||||
b3MeshDef denseDef = MakeValleyDef( vertices, indices );
|
||||
b3MeshData* denseMesh = b3CreateMesh( &denseDef, NULL, 0 );
|
||||
ENSURE( denseMesh != NULL );
|
||||
|
||||
FatVertex fat[VALLEY_VERTEX_COUNT];
|
||||
MakeFatVertices( fat, vertices, VALLEY_VERTEX_COUNT );
|
||||
|
||||
b3MeshDef fatDef = MakeValleyDef( &fat[0].position, indices );
|
||||
fatDef.stride = sizeof( FatVertex );
|
||||
b3MeshData* fatMesh = b3CreateMesh( &fatDef, NULL, 0 );
|
||||
ENSURE( fatMesh != NULL );
|
||||
|
||||
ENSURE( VerticesMatch( fatMesh, vertices, VALLEY_VERTEX_COUNT ) );
|
||||
ENSURE( FacesUp( fatMesh ) );
|
||||
ENSURE( MeshesMatch( denseMesh, fatMesh ) );
|
||||
|
||||
b3DestroyMesh( fatMesh );
|
||||
b3DestroyMesh( denseMesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Welding reads the source vertices through the stride as well
|
||||
static int MeshStrideWeld( void )
|
||||
{
|
||||
b3Vec3 vertices[VALLEY_VERTEX_COUNT];
|
||||
int32_t indices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
MakeValley( vertices, indices );
|
||||
|
||||
// Split the crease so each quad owns a copy of the shared edge
|
||||
b3Vec3 splitVertices[8];
|
||||
memcpy( splitVertices, vertices, sizeof( vertices ) );
|
||||
splitVertices[6] = vertices[1];
|
||||
splitVertices[7] = vertices[4];
|
||||
|
||||
// Point the right quad at the copies
|
||||
int32_t splitIndices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
memcpy( splitIndices, indices, sizeof( indices ) );
|
||||
splitIndices[6] = 6;
|
||||
splitIndices[7] = 7;
|
||||
splitIndices[9] = 7;
|
||||
|
||||
FatVertex fat[8];
|
||||
MakeFatVertices( fat, splitVertices, 8 );
|
||||
|
||||
b3MeshDef def = MakeValleyDef( &fat[0].position, splitIndices );
|
||||
def.stride = sizeof( FatVertex );
|
||||
def.vertexCount = 8;
|
||||
def.weldVertices = true;
|
||||
def.weldTolerance = 0.01f;
|
||||
|
||||
b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 );
|
||||
ENSURE( mesh != NULL );
|
||||
|
||||
ENSURE( mesh->vertexCount == VALLEY_VERTEX_COUNT );
|
||||
ENSURE( mesh->triangleCount == VALLEY_TRIANGLE_COUNT );
|
||||
ENSURE( VerticesMatch( mesh, vertices, VALLEY_VERTEX_COUNT ) );
|
||||
ENSURE( FacesUp( mesh ) );
|
||||
ENSURE( HasConcaveEdge( mesh ) );
|
||||
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Clockwise input plus the flag must bake to the same mesh as counter clockwise input
|
||||
static int MeshClockWise( void )
|
||||
{
|
||||
b3Vec3 vertices[VALLEY_VERTEX_COUNT];
|
||||
int32_t indices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
MakeValley( vertices, indices );
|
||||
|
||||
b3MeshDef ccwDef = MakeValleyDef( vertices, indices );
|
||||
b3MeshData* ccwMesh = b3CreateMesh( &ccwDef, NULL, 0 );
|
||||
ENSURE( ccwMesh != NULL );
|
||||
|
||||
int32_t reversed[3 * VALLEY_TRIANGLE_COUNT];
|
||||
memcpy( reversed, indices, sizeof( indices ) );
|
||||
ReverseWinding( reversed, VALLEY_TRIANGLE_COUNT );
|
||||
|
||||
b3MeshDef cwDef = MakeValleyDef( vertices, reversed );
|
||||
cwDef.clockWiseWinding = true;
|
||||
b3MeshData* cwMesh = b3CreateMesh( &cwDef, NULL, 0 );
|
||||
ENSURE( cwMesh != NULL );
|
||||
|
||||
ENSURE( FacesUp( cwMesh ) );
|
||||
ENSURE( HasConcaveEdge( cwMesh ) );
|
||||
ENSURE( MeshesMatch( ccwMesh, cwMesh ) );
|
||||
|
||||
b3DestroyMesh( cwMesh );
|
||||
b3DestroyMesh( ccwMesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Without the flag the same clockwise input must bake inside out
|
||||
static int MeshClockWiseIgnored( void )
|
||||
{
|
||||
b3Vec3 vertices[VALLEY_VERTEX_COUNT];
|
||||
int32_t indices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
MakeValley( vertices, indices );
|
||||
ReverseWinding( indices, VALLEY_TRIANGLE_COUNT );
|
||||
|
||||
b3MeshDef def = MakeValleyDef( vertices, indices );
|
||||
b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 );
|
||||
ENSURE( mesh != NULL );
|
||||
|
||||
ENSURE( FacesUp( mesh ) == false );
|
||||
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Winding, stride and welding have to compose
|
||||
static int MeshClockWiseStrideWeld( void )
|
||||
{
|
||||
b3Vec3 vertices[VALLEY_VERTEX_COUNT];
|
||||
int32_t indices[3 * VALLEY_TRIANGLE_COUNT];
|
||||
MakeValley( vertices, indices );
|
||||
|
||||
b3MeshDef ccwDef = MakeValleyDef( vertices, indices );
|
||||
ccwDef.weldVertices = true;
|
||||
ccwDef.weldTolerance = 0.01f;
|
||||
b3MeshData* ccwMesh = b3CreateMesh( &ccwDef, NULL, 0 );
|
||||
ENSURE( ccwMesh != NULL );
|
||||
|
||||
int32_t reversed[3 * VALLEY_TRIANGLE_COUNT];
|
||||
memcpy( reversed, indices, sizeof( indices ) );
|
||||
ReverseWinding( reversed, VALLEY_TRIANGLE_COUNT );
|
||||
|
||||
FatVertex fat[VALLEY_VERTEX_COUNT];
|
||||
MakeFatVertices( fat, vertices, VALLEY_VERTEX_COUNT );
|
||||
|
||||
b3MeshDef cwDef = MakeValleyDef( &fat[0].position, reversed );
|
||||
cwDef.stride = sizeof( FatVertex );
|
||||
cwDef.clockWiseWinding = true;
|
||||
cwDef.weldVertices = true;
|
||||
cwDef.weldTolerance = 0.01f;
|
||||
b3MeshData* cwMesh = b3CreateMesh( &cwDef, NULL, 0 );
|
||||
ENSURE( cwMesh != NULL );
|
||||
|
||||
ENSURE( FacesUp( cwMesh ) );
|
||||
ENSURE( MeshesMatch( ccwMesh, cwMesh ) );
|
||||
|
||||
b3DestroyMesh( cwMesh );
|
||||
b3DestroyMesh( ccwMesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The built in creators fill their own def, so a missed stride shows up here
|
||||
static int MeshCreators( void )
|
||||
{
|
||||
b3Vec3 center = { 1.0f, 2.0f, 3.0f };
|
||||
b3Vec3 extent = { 0.5f, 1.0f, 1.5f };
|
||||
|
||||
b3MeshData* meshes[] = {
|
||||
b3CreateGridMesh( 4, 4, 1.0f, 2, true ),
|
||||
b3CreateWaveMesh( 4, 4, 1.0f, 0.5f, 1.0f, 1.0f ),
|
||||
b3CreateTorusMesh( 8, 6, 2.0f, 0.5f ),
|
||||
b3CreateBoxMesh( center, extent, true ),
|
||||
b3CreateHollowBoxMesh( center, extent ),
|
||||
b3CreatePlatformMesh( center, 2.0f, 1.0f, 2.0f ),
|
||||
};
|
||||
|
||||
for ( int i = 0; i < ARRAY_COUNT( meshes ); ++i )
|
||||
{
|
||||
ENSURE( meshes[i] != NULL );
|
||||
ENSURE( meshes[i]->vertexCount >= 3 );
|
||||
ENSURE( meshes[i]->triangleCount >= 1 );
|
||||
ENSURE( b3IsSaneAABB( meshes[i]->bounds ) );
|
||||
|
||||
b3DestroyMesh( meshes[i] );
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MeshTest( void )
|
||||
{
|
||||
RUN_SUBTEST( MeshDenseStride );
|
||||
RUN_SUBTEST( MeshFatStride );
|
||||
RUN_SUBTEST( MeshStrideWeld );
|
||||
RUN_SUBTEST( MeshClockWise );
|
||||
RUN_SUBTEST( MeshClockWiseIgnored );
|
||||
RUN_SUBTEST( MeshClockWiseStrideWeld );
|
||||
RUN_SUBTEST( MeshCreators );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -6,6 +6,7 @@
|
|||
// b3CollideMoverAndSphere / Capsule / Hull are internal
|
||||
#include "shape.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
#include "box3d/collision.h"
|
||||
|
||||
static int ParallelPlanes( void )
|
||||
|
|
@ -54,15 +55,11 @@ static int GamePlanes( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Mover-collide overlap handling
|
||||
//
|
||||
// b3CollideMoverAndSphere / Capsule / Hull must never emit a plane with a
|
||||
// degenerate (zero) normal, even when the mover deeply penetrates the shape.
|
||||
// On deep overlap the GJK path returns a {0,0,0} normal; these tests guard the
|
||||
// fix that replaces it with an analytic (sphere/capsule) or dropped (hull) result.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static int MoverSphereSeparated( void )
|
||||
{
|
||||
b3Sphere shape = { { 0.0f, 0.0f, 0.0f }, 0.5f };
|
||||
|
|
@ -233,6 +230,467 @@ static int MoverHullDeepOverlap( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Mover queries report which material a contact plane came from
|
||||
// b3PlaneResult::materialIndex follows different paths per shape type. Meshes
|
||||
// report the per triangle material index. Compounds remap the child result
|
||||
// through the child material table. Convex shapes report index 0.
|
||||
static b3SurfaceMaterial MakeMaterial( float friction, uint64_t userId )
|
||||
{
|
||||
b3SurfaceMaterial m = b3DefaultSurfaceMaterial();
|
||||
m.friction = friction;
|
||||
m.userMaterialId = userId;
|
||||
return m;
|
||||
}
|
||||
|
||||
// Two separated upward facing triangles on the y = 0 plane, one per material. The bake may
|
||||
// reorder triangles but always keeps a triangle paired with its material index.
|
||||
static b3MeshData* MakeTwoMaterialMesh( void )
|
||||
{
|
||||
b3Vec3 vertices[6] = {
|
||||
{ -3.0f, 0.0f, -1.0f }, { -2.0f, 0.0f, 1.0f }, { -1.0f, 0.0f, -1.0f },
|
||||
{ 1.0f, 0.0f, -1.0f }, { 2.0f, 0.0f, 1.0f }, { 3.0f, 0.0f, -1.0f },
|
||||
};
|
||||
int32_t indices[6] = { 0, 1, 2, 3, 4, 5 };
|
||||
uint8_t materialIndices[2] = { 0, 1 };
|
||||
|
||||
b3MeshDef def = { 0 };
|
||||
def.vertices = vertices;
|
||||
def.stride = sizeof( b3Vec3 );
|
||||
def.indices = indices;
|
||||
def.materialIndices = materialIndices;
|
||||
def.vertexCount = 6;
|
||||
def.triangleCount = 2;
|
||||
|
||||
return b3CreateMesh( &def, NULL, 0 );
|
||||
}
|
||||
|
||||
// Flat 3x3 vertex field at y = 0, one cell material per entry. Caller destroys.
|
||||
static b3HeightFieldData* MakeFlatField( uint8_t* materials, bool clockwise )
|
||||
{
|
||||
float heights[9] = { 0 };
|
||||
|
||||
b3HeightFieldDef def = { 0 };
|
||||
def.heights = heights;
|
||||
def.materialIndices = materials;
|
||||
def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f };
|
||||
def.countX = 3;
|
||||
def.countZ = 3;
|
||||
def.globalMinimumHeight = -1.0f;
|
||||
def.globalMaximumHeight = 1.0f;
|
||||
def.clockwiseWinding = clockwise;
|
||||
|
||||
return b3CreateHeightField( &def );
|
||||
}
|
||||
|
||||
typedef struct PlaneCapture
|
||||
{
|
||||
b3PlaneResult planes[16];
|
||||
int count;
|
||||
} PlaneCapture;
|
||||
|
||||
static bool CapturePlaneFcn( b3ShapeId shapeId, const b3PlaneResult* planes, int planeCount, void* context )
|
||||
{
|
||||
(void)shapeId;
|
||||
PlaneCapture* capture = context;
|
||||
for ( int i = 0; i < planeCount && capture->count < 16; ++i )
|
||||
{
|
||||
capture->planes[capture->count++] = planes[i];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static int MoverWorldMeshMaterials( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_staticBody;
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3MeshData* mesh = MakeTwoMaterialMesh();
|
||||
ENSURE( mesh != NULL );
|
||||
ENSURE( mesh->materialCount == 2 );
|
||||
|
||||
b3SurfaceMaterial materials[2] = { MakeMaterial( 0.2f, 1 ), MakeMaterial( 0.8f, 2 ) };
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.materials = materials;
|
||||
shapeDef.materialCount = 2;
|
||||
b3CreateMeshShape( bodyId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } );
|
||||
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 1 );
|
||||
|
||||
const uint8_t* bakedMaterialIndices = b3GetMeshMaterialIndices( mesh );
|
||||
|
||||
// Mover hanging just above the first triangle so its radius reaches 0.05 into the surface
|
||||
b3Capsule mover = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
|
||||
PlaneCapture capture = { 0 };
|
||||
b3World_CollideMover( worldId, b3Pos_zero, &mover, b3DefaultQueryFilter(), CapturePlaneFcn, &capture );
|
||||
|
||||
ENSURE( capture.count == 1 );
|
||||
ENSURE( capture.planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE_SMALL( capture.planes[0].plane.offset - 0.05f, 1e-4f );
|
||||
ENSURE_SMALL( capture.planes[0].point.y, 1e-4f );
|
||||
ENSURE( capture.planes[0].materialIndex == 0 );
|
||||
ENSURE( capture.planes[0].triangleIndex >= 0 );
|
||||
ENSURE( capture.planes[0].triangleIndex < mesh->triangleCount );
|
||||
ENSURE( bakedMaterialIndices[capture.planes[0].triangleIndex] == 0 );
|
||||
|
||||
// Same mover over the second triangle
|
||||
b3Capsule mover2 = { { 2.0f, 0.15f, 0.0f }, { 2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
|
||||
PlaneCapture capture2 = { 0 };
|
||||
b3World_CollideMover( worldId, b3Pos_zero, &mover2, b3DefaultQueryFilter(), CapturePlaneFcn, &capture2 );
|
||||
|
||||
ENSURE( capture2.count == 1 );
|
||||
ENSURE( capture2.planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE( capture2.planes[0].materialIndex == 1 );
|
||||
ENSURE( bakedMaterialIndices[capture2.planes[0].triangleIndex] == 1 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverWorldCompoundMeshMaterials( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_staticBody;
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3MeshData* mesh = MakeTwoMaterialMesh();
|
||||
|
||||
// The hull material and the two mesh materials are distinct, so the compound material
|
||||
// table gets one slot per material. Children bake in hull then mesh order.
|
||||
b3SurfaceMaterial meshMaterials[2] = { MakeMaterial( 0.3f, 101 ), MakeMaterial( 0.6f, 202 ) };
|
||||
b3SurfaceMaterial hullMaterial = MakeMaterial( 0.9f, 303 );
|
||||
|
||||
b3CompoundMeshDef meshChild = {
|
||||
.meshData = mesh,
|
||||
.transform = b3Transform_identity,
|
||||
.scale = { 1.0f, 1.0f, 1.0f },
|
||||
.materials = meshMaterials,
|
||||
.materialCount = 2,
|
||||
};
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3CompoundHullDef hullChild = {
|
||||
.hull = &box.base,
|
||||
.transform = { .p = { -8.0f, 0.0f, 0.0f }, .q = b3Quat_identity },
|
||||
.material = hullMaterial,
|
||||
};
|
||||
|
||||
b3CompoundDef compoundDef = {
|
||||
.hulls = &hullChild,
|
||||
.hullCount = 1,
|
||||
.meshes = &meshChild,
|
||||
.meshCount = 1,
|
||||
};
|
||||
b3CompoundData* compound = b3CreateCompound( &compoundDef );
|
||||
ENSURE( compound != NULL );
|
||||
ENSURE( compound->materialCount == 3 );
|
||||
|
||||
const b3SurfaceMaterial* bakedMaterials = b3GetCompoundMaterials( compound );
|
||||
ENSURE( bakedMaterials[0].userMaterialId == 303 );
|
||||
ENSURE( bakedMaterials[1].userMaterialId == 101 );
|
||||
ENSURE( bakedMaterials[2].userMaterialId == 202 );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateBakedCompoundShape( bodyId, &shapeDef, compound );
|
||||
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 1 );
|
||||
|
||||
// Mover on top of the hull child face at y = 0.5
|
||||
b3Capsule hullMover = { { -8.0f, 0.65f, 0.0f }, { -8.0f, 0.85f, 0.0f }, 0.2f };
|
||||
|
||||
PlaneCapture hullCapture = { 0 };
|
||||
b3World_CollideMover( worldId, b3Pos_zero, &hullMover, b3DefaultQueryFilter(), CapturePlaneFcn, &hullCapture );
|
||||
|
||||
ENSURE( hullCapture.count == 1 );
|
||||
ENSURE( hullCapture.planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE( hullCapture.planes[0].childIndex == 0 );
|
||||
ENSURE( hullCapture.planes[0].materialIndex == 0 );
|
||||
ENSURE( bakedMaterials[hullCapture.planes[0].materialIndex].userMaterialId == 303 );
|
||||
|
||||
// Mover over the second mesh triangle. The mesh reports triangle material 1, which the
|
||||
// compound remaps through the child material table to the shared slot of meshMaterials[1].
|
||||
b3Capsule meshMover = { { 2.0f, 0.15f, 0.0f }, { 2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
|
||||
PlaneCapture meshCapture = { 0 };
|
||||
b3World_CollideMover( worldId, b3Pos_zero, &meshMover, b3DefaultQueryFilter(), CapturePlaneFcn, &meshCapture );
|
||||
|
||||
ENSURE( meshCapture.count == 1 );
|
||||
ENSURE( meshCapture.planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE( meshCapture.planes[0].childIndex == 1 );
|
||||
ENSURE( meshCapture.planes[0].materialIndex == 2 );
|
||||
ENSURE( bakedMaterials[meshCapture.planes[0].materialIndex].userMaterialId == 202 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
b3DestroyCompound( compound );
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverBodyMaterialIndices( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
// One shape of each convex type, spaced out along X
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
||||
b3Sphere sphere = { { 0.0f, 0.5f, 0.0f }, 0.5f };
|
||||
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
|
||||
|
||||
b3BoxHull box = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f,
|
||||
(b3Transform){ .p = { 5.0f, 0.0f, 0.0f }, .q = b3Quat_identity } );
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
b3Capsule capsule = { { 9.0f, 0.0f, 0.0f }, { 11.0f, 0.0f, 0.0f }, 0.3f };
|
||||
b3CreateCapsuleShape( bodyId, &shapeDef, &capsule );
|
||||
|
||||
b3WorldTransform bodyTransform = { .p = b3Pos_zero, .q = b3Quat_identity };
|
||||
|
||||
// Convex shapes report the base material as index 0
|
||||
b3Capsule movers[3] = {
|
||||
{ { 0.0f, 1.15f, 0.0f }, { 0.0f, 1.35f, 0.0f }, 0.2f },
|
||||
{ { 5.0f, 0.65f, 0.0f }, { 5.0f, 0.85f, 0.0f }, 0.2f },
|
||||
{ { 10.0f, 0.45f, 0.0f }, { 10.0f, 0.65f, 0.0f }, 0.2f },
|
||||
};
|
||||
|
||||
for ( int i = 0; i < 3; ++i )
|
||||
{
|
||||
b3BodyPlaneResult planes[4];
|
||||
int count = b3Body_CollideMover( bodyId, planes, 4, b3Pos_zero, &movers[i], b3DefaultQueryFilter(), bodyTransform );
|
||||
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( b3Shape_IsValid( planes[0].shapeId ) );
|
||||
ENSURE( planes[0].result.plane.normal.y > 0.99f );
|
||||
ENSURE( planes[0].result.materialIndex == 0 );
|
||||
ENSURE( planes[0].result.childIndex == 0 );
|
||||
}
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverBodySkipsMeshAndCompound( void )
|
||||
{
|
||||
// The body level mover query handles convex shapes only. Mesh and compound shapes are
|
||||
// skipped, so their material indices are only available through b3World_CollideMover.
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
|
||||
b3MeshData* mesh = MakeTwoMaterialMesh();
|
||||
ENSURE( mesh != NULL );
|
||||
b3CreateMeshShape( bodyId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
b3CompoundHullDef hullChild = {
|
||||
.hull = &box.base,
|
||||
.transform = { .p = { 10.0f, 0.0f, 0.0f }, .q = b3Quat_identity },
|
||||
};
|
||||
b3CompoundDef compoundDef = {
|
||||
.hulls = &hullChild,
|
||||
.hullCount = 1,
|
||||
};
|
||||
b3CompoundData* compound = b3CreateCompound( &compoundDef );
|
||||
ENSURE( compound != NULL );
|
||||
b3CreateBakedCompoundShape( bodyId, &shapeDef, compound );
|
||||
|
||||
b3WorldTransform bodyTransform = { .p = b3Pos_zero, .q = b3Quat_identity };
|
||||
|
||||
// Mover over the mesh triangle and mover over the compound hull child both find nothing
|
||||
b3Capsule meshMover = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
b3Capsule compoundMover = { { 10.0f, 0.65f, 0.0f }, { 10.0f, 0.85f, 0.0f }, 0.2f };
|
||||
|
||||
b3Capsule movers[2] = { meshMover, compoundMover };
|
||||
for ( int i = 0; i < 2; ++i )
|
||||
{
|
||||
b3BodyPlaneResult planes[4];
|
||||
int count = b3Body_CollideMover( bodyId, planes, 4, b3Pos_zero, &movers[i], b3DefaultQueryFilter(), bodyTransform );
|
||||
ENSURE( count == 0 );
|
||||
}
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
b3DestroyCompound( compound );
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// One sided mover collision.
|
||||
// Mover queries keep only triangles facing the mover. The front side follows the
|
||||
// baked winding: up for a default mesh or height field, down when the height
|
||||
// field carries clockwiseWinding. A mirror in the mesh scale must not flip it.
|
||||
static int MoverMeshBackside( void )
|
||||
{
|
||||
b3MeshData* mesh = MakeTwoMaterialMesh();
|
||||
b3Mesh shape = { .data = mesh, .scale = { 1.0f, 1.0f, 1.0f } };
|
||||
|
||||
b3PlaneResult planes[4];
|
||||
|
||||
// On the front of the left triangle, so a plane comes back
|
||||
b3Capsule above = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
int count = b3CollideMoverAndMesh( planes, 4, &shape, &above );
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE( planes[0].childIndex == 0 );
|
||||
ENSURE( planes[0].triangleIndex >= 0 && planes[0].triangleIndex < mesh->triangleCount );
|
||||
|
||||
// The same spot seen from behind the face is culled
|
||||
b3Capsule below = { { -2.0f, -0.35f, 0.0f }, { -2.0f, -0.15f, 0.0f }, 0.2f };
|
||||
count = b3CollideMoverAndMesh( planes, 4, &shape, &below );
|
||||
ENSURE( count == 0 );
|
||||
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverMeshMirroredScale( void )
|
||||
{
|
||||
b3MeshData* mesh = MakeTwoMaterialMesh();
|
||||
|
||||
// Reflecting the scale flips the triangle winding, the collision swaps it back,
|
||||
// so the front stays up. Local x = -2 maps onto the source right triangle, material 1.
|
||||
b3Mesh shape = { .data = mesh, .scale = { -1.0f, 1.0f, 1.0f } };
|
||||
|
||||
b3PlaneResult planes[4];
|
||||
|
||||
b3Capsule above = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
int count = b3CollideMoverAndMesh( planes, 4, &shape, &above );
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE( planes[0].materialIndex == 1 );
|
||||
ENSURE( b3GetMeshMaterialIndices( mesh )[planes[0].triangleIndex] == 1 );
|
||||
|
||||
b3Capsule below = { { -2.0f, -0.35f, 0.0f }, { -2.0f, -0.15f, 0.0f }, 0.2f };
|
||||
count = b3CollideMoverAndMesh( planes, 4, &shape, &below );
|
||||
ENSURE( count == 0 );
|
||||
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverHeightFieldBackside( void )
|
||||
{
|
||||
uint8_t materials[4] = { 0, 0, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, false );
|
||||
|
||||
b3PlaneResult planes[4];
|
||||
|
||||
// Standing on the front (upper) face of the default winding
|
||||
b3Capsule above = { { 0.3f, 0.15f, 0.25f }, { 0.3f, 0.35f, 0.25f }, 0.2f };
|
||||
int count = b3CollideMoverAndHeightField( planes, 4, hf, &above );
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( planes[0].plane.normal.y > 0.99f );
|
||||
ENSURE_SMALL( planes[0].plane.offset - 0.05f, 1e-4f );
|
||||
|
||||
// Under the surface is the back side and gets culled
|
||||
b3Capsule below = { { 0.3f, -0.35f, 0.25f }, { 0.3f, -0.15f, 0.25f }, 0.2f };
|
||||
count = b3CollideMoverAndHeightField( planes, 4, hf, &below );
|
||||
ENSURE( count == 0 );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int MoverHeightFieldReport( void )
|
||||
{
|
||||
uint8_t materials[4] = { 1, 2, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, false );
|
||||
|
||||
b3PlaneResult planes[4];
|
||||
|
||||
// (0.3, 0.25) sits on the x + z <= 1 side of cell (0,0), which holds triangle 0
|
||||
b3Capsule first = { { 0.3f, 0.15f, 0.25f }, { 0.3f, 0.35f, 0.25f }, 0.2f };
|
||||
int count = b3CollideMoverAndHeightField( planes, 4, hf, &first );
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( planes[0].triangleIndex == 0 );
|
||||
ENSURE( planes[0].childIndex == 0 );
|
||||
ENSURE( planes[0].materialIndex == 1 );
|
||||
|
||||
// (1.3, 0.3) sits on the x + z <= 2 side of cell (0,1), which holds triangle 2
|
||||
b3Capsule second = { { 1.3f, 0.15f, 0.3f }, { 1.3f, 0.35f, 0.3f }, 0.2f };
|
||||
count = b3CollideMoverAndHeightField( planes, 4, hf, &second );
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( planes[0].triangleIndex == 2 );
|
||||
ENSURE( planes[0].materialIndex == 2 );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A clockwise height field faces down, see HeightFieldWinding. Backside culling
|
||||
// must respect the flag: below the surface is the front side, above is the back.
|
||||
static int MoverHeightFieldClockwise( void )
|
||||
{
|
||||
uint8_t materials[4] = { 0, 0, 0, 0 };
|
||||
b3HeightFieldData* hf = MakeFlatField( materials, true );
|
||||
|
||||
b3PlaneResult planes[4];
|
||||
|
||||
b3Capsule below = { { 0.3f, -0.35f, 0.25f }, { 0.3f, -0.15f, 0.25f }, 0.2f };
|
||||
int count = b3CollideMoverAndHeightField( planes, 4, hf, &below );
|
||||
ENSURE( count == 1 );
|
||||
ENSURE( planes[0].plane.normal.y < -0.99f );
|
||||
ENSURE_SMALL( planes[0].plane.offset - 0.05f, 1e-4f );
|
||||
ENSURE( planes[0].triangleIndex == 0 || planes[0].triangleIndex == 1 );
|
||||
|
||||
b3Capsule above = { { 0.3f, 0.15f, 0.25f }, { 0.3f, 0.35f, 0.25f }, 0.2f };
|
||||
count = b3CollideMoverAndHeightField( planes, 4, hf, &above );
|
||||
ENSURE( count == 0 );
|
||||
|
||||
b3DestroyHeightField( hf );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A mesh can be baked with more materials than the shape carries. The reported index must stay
|
||||
// inside the shape's material array.
|
||||
static int MoverWorldMeshMaterialClamp( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_staticBody;
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3MeshData* mesh = MakeTwoMaterialMesh();
|
||||
ENSURE( mesh->materialCount == 2 );
|
||||
|
||||
// Base material only
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3ShapeId shapeId = b3CreateMeshShape( bodyId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } );
|
||||
int shapeMaterialCount = b3Shape_GetMeshMaterialCount( shapeId );
|
||||
ENSURE( shapeMaterialCount == 1 );
|
||||
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 1 );
|
||||
|
||||
// Over the triangle baked with material 1
|
||||
b3Capsule mover = { { 2.0f, 0.15f, 0.0f }, { 2.0f, 0.35f, 0.0f }, 0.2f };
|
||||
PlaneCapture capture = { 0 };
|
||||
b3World_CollideMover( worldId, b3Pos_zero, &mover, b3DefaultQueryFilter(), CapturePlaneFcn, &capture );
|
||||
|
||||
ENSURE( capture.count == 1 );
|
||||
ENSURE( capture.planes[0].materialIndex == shapeMaterialCount - 1 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
b3DestroyMesh( mesh );
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MoverTest( void )
|
||||
{
|
||||
RUN_SUBTEST( GamePlanes );
|
||||
|
|
@ -251,5 +709,16 @@ int MoverTest( void )
|
|||
RUN_SUBTEST( MoverHullTouching );
|
||||
RUN_SUBTEST( MoverHullDeepOverlap );
|
||||
|
||||
RUN_SUBTEST( MoverWorldMeshMaterials );
|
||||
RUN_SUBTEST( MoverWorldCompoundMeshMaterials );
|
||||
RUN_SUBTEST( MoverBodyMaterialIndices );
|
||||
RUN_SUBTEST( MoverBodySkipsMeshAndCompound );
|
||||
RUN_SUBTEST( MoverMeshBackside );
|
||||
RUN_SUBTEST( MoverMeshMirroredScale );
|
||||
RUN_SUBTEST( MoverHeightFieldBackside );
|
||||
RUN_SUBTEST( MoverHeightFieldReport );
|
||||
RUN_SUBTEST( MoverHeightFieldClockwise );
|
||||
RUN_SUBTEST( MoverWorldMeshMaterialClamp );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue