box3d/samples/sample_collision.cpp
Erin Catto a252f03b19
Hull sharing (#13)
Save memory when there are many duplicate hulls. Can improve performance
in some cases.

For consistency with b3MeshData, b3Hull is renamed to b3HullData.
2026-06-11 22:39:26 -07:00

2568 lines
68 KiB
C++

// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "sample.h"
#include "gfx/draw.h"
#include "utils.h"
#include "gfx/debug_adapter.h"
#include "gfx/keycodes.h"
#include "box3d/box3d.h"
#include <imgui.h>
class RayCurtain : public Sample
{
public:
static Sample* Create( SampleContext* context )
{
return new RayCurtain( context );
}
explicit RayCurtain( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 20.0f );
}
b3BoxHull box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f );
m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_kinematicBody;
b3ShapeDef shapeDef = b3DefaultShapeDef();
bodyDef.position = { -6.0f, 3.0f, 0.0f };
bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f };
b3BodyId sphereBody = b3CreateBody( m_worldId, &bodyDef );
b3Sphere sphere = { b3Vec3_zero, 0.9f };
b3CreateSphereShape( sphereBody, &shapeDef, &sphere );
bodyDef.position = { -2.0f, 3.0f, 0.0f };
bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f };
b3BodyId capsuleBody = b3CreateBody( m_worldId, &bodyDef );
b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.8f };
b3CreateCapsuleShape( capsuleBody, &shapeDef, &capsule );
bodyDef.position = { 2.0f, 3.0f, 0.0f };
bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f };
b3BodyId hullBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( hullBody, &shapeDef, &box.base );
bodyDef.position = { 6.0f, 3.0f, 0.0f };
bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f };
b3BodyId meshBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateMeshShape( meshBody, &shapeDef, m_mesh, b3Vec3_one );
m_absSpeed = 0.015f;
m_offset = 2.0f;
m_speed = -m_absSpeed;
}
~RayCurtain() override
{
b3DestroyMesh( m_mesh );
}
void Render() override
{
Sample::Render();
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
for ( float x = -8.0f; x <= 8.0f; x += 0.1f )
{
b3Vec3 rayOrigin = { x, 8.0f, m_offset };
b3Vec3 rayTranslation = b3Vec3{ x, 0.0f, m_offset } - rayOrigin;
b3RayResult result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() );
if ( result.hit )
{
b3Vec3 point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGreen ) );
}
DrawPoint( rayOrigin, 4.0f, MakeColor( b3_colorGreen ) );
DrawPoint( rayOrigin + rayTranslation, 4.0f, MakeColor( b3_colorRed ) );
DrawLine( rayOrigin, rayOrigin + rayTranslation, MakeColor( b3_colorYellow ) );
}
if ( m_offset > 2.0f )
{
m_speed = -m_absSpeed;
}
else if ( m_offset < -2.0f )
{
m_speed = m_absSpeed;
}
if ( m_context->pause == false )
{
m_offset += m_speed;
}
}
float m_offset;
float m_absSpeed;
float m_speed;
b3MeshData* m_mesh;
};
static int sampleRayCurtain = RegisterSample( "Collision", "Ray Curtain", RayCurtain::Create );
// Context for ray cast callbacks. Do what you want with this.
struct CastContext
{
b3Vec3 points[3];
b3Vec3 normals[3];
float fractions[3];
uint64_t materialIds[3];
int triangleIndices[3];
int childIndices[3];
int count;
bool initialOverlap;
};
// This callback finds the closest hit. This is the most common callback used in games.
static float RayCastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
// Check for initial overlap
if ( rayContext->initialOverlap == false && fraction == 0.0f )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
// Ignore a specific shape. Also ignore initial overlap.
intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) );
if ( ignoreFlag == 1 )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
rayContext->points[0] = point;
rayContext->normals[0] = normal;
rayContext->fractions[0] = fraction;
rayContext->materialIds[0] = materialId;
rayContext->triangleIndices[0] = triangleIndex;
rayContext->childIndices[0] = childIndex;
rayContext->count = 1;
// By returning the current fraction, we instruct the calling code to clip the ray and
// continue the ray-cast to the next shape. WARNING: do not assume that shapes
// are reported in order. However, by clipping, we can always get the closest shape.
return fraction;
}
// This callback finds any hit. For this type of query we are usually just checking for obstruction,
// so the hit data is not relevant.
// NOTE: shape hits are not ordered, so this may not return the closest hit
static float RayCastAnyCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
// Check for initial overlap
if ( rayContext->initialOverlap == false && fraction == 0.0f )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) );
if ( ignoreFlag == 1 )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
rayContext->points[0] = point;
rayContext->normals[0] = normal;
rayContext->fractions[0] = fraction;
rayContext->materialIds[0] = materialId;
rayContext->triangleIndices[0] = triangleIndex;
rayContext->childIndices[0] = childIndex;
rayContext->count = 1;
// At this point we have a hit, so we know the ray is obstructed.
// By returning 0, we instruct the calling code to terminate the ray-cast.
return 0.0f;
}
// This ray cast collects multiple hits along the ray.
// The shapes are not necessary reported in order, so we might not capture
// the closest shape.
// NOTE: shape hits are not ordered, so this may return hits in any order. This means that
// if you limit the number of results, you may discard the closest hit. You can see this
// behavior in the sample.
static float RayCastMultipleCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
// Check for initial overlap
if ( rayContext->initialOverlap == false && fraction == 0.0f )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) );
if ( ignoreFlag == 1 )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
int count = rayContext->count;
assert( count < 3 );
rayContext->points[count] = point;
rayContext->normals[count] = normal;
rayContext->fractions[count] = fraction;
rayContext->materialIds[count] = materialId;
rayContext->triangleIndices[count] = triangleIndex;
rayContext->childIndices[count] = childIndex;
rayContext->count = count + 1;
if ( rayContext->count == 3 )
{
// At this point the buffer is full.
// By returning 0, we instruct the calling code to terminate the ray-cast.
return 0.0f;
}
// By returning 1, we instruct the caller to continue without clipping the ray.
return 1.0f;
}
// This ray cast collects multiple hits along the ray and sorts them.
static float RayCastSortedCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
// Check for initial overlap
if ( rayContext->initialOverlap == false && fraction == 0.0f )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) );
if ( ignoreFlag == 1 )
{
// By returning -1, we instruct the calling code to ignore this shape and
// continue the ray-cast to the next shape.
return -1.0f;
}
int count = rayContext->count;
assert( count <= 3 );
int index = 3;
while ( fraction < rayContext->fractions[index - 1] )
{
index -= 1;
if ( index == 0 )
{
break;
}
}
if ( index == 3 )
{
// not closer, continue but tell the caller not to consider fractions further than the largest fraction acquired
// this only happens once the buffer is full
assert( rayContext->count == 3 );
assert( rayContext->fractions[2] <= 1.0f );
return rayContext->fractions[2];
}
for ( int j = 2; j > index; --j )
{
rayContext->points[j] = rayContext->points[j - 1];
rayContext->normals[j] = rayContext->normals[j - 1];
rayContext->fractions[j] = rayContext->fractions[j - 1];
rayContext->materialIds[j] = rayContext->materialIds[j - 1];
rayContext->triangleIndices[j] = rayContext->triangleIndices[j - 1];
rayContext->childIndices[j] = rayContext->childIndices[j - 1];
}
rayContext->points[index] = point;
rayContext->normals[index] = normal;
rayContext->fractions[index] = fraction;
rayContext->materialIds[index] = materialId;
rayContext->triangleIndices[index] = triangleIndex;
rayContext->childIndices[index] = childIndex;
rayContext->count = count < 3 ? count + 1 : 3;
if ( rayContext->count == 3 )
{
return rayContext->fractions[2];
}
// By returning 1, we instruct the caller to continue without clipping the ray.
return 1.0f;
}
class CastWorld : public Sample
{
public:
enum
{
e_any = 0,
e_closest = 1,
e_multiple = 2,
e_sorted = 3
};
enum
{
e_rayCast = 0,
e_sphereCast = 1,
e_capsuleCast = 2,
e_boxCast = 3
};
enum
{
e_maxCount = 64
};
static Sample* Create( SampleContext* context )
{
return new CastWorld( context );
}
explicit CastWorld( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 20.0f );
}
m_sphere = { b3Vec3_zero, 0.9f };
m_capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.8f };
m_box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f );
m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f );
m_heightField = b3CreateWave( 10, 10, 0.5f * b3Vec3_one, 0.03f, 0.09f, false );
m_bodyIndex = 0;
for ( int i = 0; i < e_maxCount; ++i )
{
m_bodies[i] = {};
}
m_mode = e_closest;
m_castType = e_rayCast;
m_castRadius = 0.5f;
m_origin = { -20.0f, 10.0f, 0.0f };
m_translation = { 20.0f, 10.0f, 0.0f };
m_castContext = {};
m_initialOverlap = false;
}
~CastWorld() override
{
b3DestroyMesh( m_mesh );
b3DestroyHeightField( m_heightField );
}
void CreateShapes( b3ShapeType shapeType, int count )
{
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.gravityScale = 0.0f;
for ( int i = 0; i < count; ++i )
{
if ( B3_IS_NULL( m_bodies[m_bodyIndex] ) == false )
{
b3DestroyBody( m_bodies[m_bodyIndex] );
m_bodies[m_bodyIndex] = b3_nullBodyId;
}
if ( m_bodyIndex % 3 == 0 )
{
bodyDef.type = b3_kinematicBody;
}
else if ( m_bodyIndex % 2 == 0 )
{
bodyDef.type = b3_dynamicBody;
}
else
{
bodyDef.type = b3_staticBody;
}
// Heightfield must be a static body
if (shapeType == b3_heightShape)
{
bodyDef.type = b3_staticBody;
}
bodyDef.position = RandomVec3Uniform( -20.0f, 20.0f );
b3Vec3 axis = RandomVec3Uniform( -1.0f, 1.0f );
axis = b3Normalize( axis );
float angle = RandomFloatRange( -B3_PI, B3_PI );
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
int flag = ( m_bodyIndex & m_ignoreBase ) == m_ignoreBase ? 1 : 0;
shapeDef.userData = (void*)(intptr_t)flag;
switch ( shapeType )
{
case b3_sphereShape:
shapeDef.baseMaterial.userMaterialId = 11;
b3CreateSphereShape( bodyId, &shapeDef, &m_sphere );
break;
case b3_capsuleShape:
shapeDef.baseMaterial.userMaterialId = 22;
b3CreateCapsuleShape( bodyId, &shapeDef, &m_capsule );
break;
case b3_hullShape:
shapeDef.baseMaterial.userMaterialId = 33;
b3CreateHullShape( bodyId, &shapeDef, &m_box.base );
break;
case b3_meshShape:
shapeDef.baseMaterial.userMaterialId = 44;
b3CreateMeshShape( bodyId, &shapeDef, m_mesh, { 4.0f, 3.0f, -2.0f } );
break;
case b3_heightShape:
{
shapeDef.baseMaterial.userMaterialId = 55;
b3SurfaceMaterial materials[3] = {
{
.userMaterialId = 111,
},
{
.userMaterialId = 222,
},
{
.userMaterialId = 333,
},
};
shapeDef.materials = materials;
shapeDef.materialCount = 3;
b3CreateHeightFieldShape( bodyId, &shapeDef, m_heightField );
}
break;
default:
assert( false );
break;
}
m_bodies[m_bodyIndex] = bodyId;
m_bodyIndex = ( m_bodyIndex + 1 ) % e_maxCount;
}
}
void DestroyBody()
{
for ( int i = 0; i < e_maxCount; ++i )
{
if ( B3_IS_NULL( m_bodies[i] ) == false )
{
b3DestroyBody( m_bodies[i] );
m_bodies[i] = b3_nullBodyId;
return;
}
}
}
void MouseDown( b3Vec2 p, int button, int modifiers ) override
{
if ( button == 0 && modifiers == MOD_CTRL )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
m_origin = pickRay.origin;
m_translation = 100.0f * b3Normalize( pickRay.translation );
}
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
const char* castTypes[] = { "Ray", "Sphere", "Capsule", "Box" };
if ( ImGui::Combo( "Cast Type", &m_castType, castTypes, IM_ARRAYSIZE( castTypes ) ) )
{
}
if ( m_castType != e_rayCast && m_castType != e_boxCast )
{
ImGui::SliderFloat( "Radius", &m_castRadius, 0.1f, 2.0f, "%.1f" );
}
const char* modes[] = { "Any", "Closest", "Multiple", "Sorted" };
if ( ImGui::Combo( "Mode", &m_mode, modes, IM_ARRAYSIZE( modes ) ) )
{
}
ImGui::Checkbox( "Initial Overlap", &m_initialOverlap );
if ( ImGui::Button( "Spheres" ) )
{
CreateShapes( b3_sphereShape, 10 );
}
if ( ImGui::Button( "Capsules" ) )
{
CreateShapes( b3_capsuleShape, 10 );
}
if ( ImGui::Button( "Hulls" ) )
{
CreateShapes( b3_hullShape, 10 );
}
if ( ImGui::Button( "Meshes" ) )
{
CreateShapes( b3_meshShape, 1 );
}
if ( ImGui::Button( "Height Field" ) )
{
CreateShapes( b3_heightShape, 1 );
}
if ( ImGui::Button( "Destroy Shape" ) )
{
DestroyBody();
}
return true;
}
void Render() override
{
DrawTextLine( "Ctrl + left mouse to cast through cursor" );
DrawTextLine( "Shapes drawn in yellow boxes are ignored by the ray" );
// Outline the bodies the cast ignores
for ( int i = 0; i < m_maxCount; ++i )
{
if ( ( i & m_ignoreBase ) == m_ignoreBase && B3_IS_NULL( m_bodies[i] ) == false )
{
b3AABB bounds = b3Body_ComputeAABB( m_bodies[i] );
DrawBounds( bounds, 0.0f, MakeColor( b3_colorYellow ) );
}
}
switch ( m_mode )
{
case e_any:
DrawTextLine( "Cast mode: any - check for obstruction - unsorted" );
break;
case e_closest:
DrawTextLine( "Cast mode: closest - find closest shape along the cast" );
break;
case e_multiple:
DrawTextLine( "Cast mode: multiple - gather multiple shapes - unsorted" );
break;
case e_sorted:
DrawTextLine( "Cast mode: sorted - gather multiple shapes sorted by closeness" );
break;
default:
assert( false );
break;
}
for ( int i = 0; i < m_castContext.count; ++i )
{
DrawTextLine( "material = %d, triangle = %d", m_castContext.materialIds[i], m_castContext.triangleIndices[i] );
}
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
Sample::Render();
}
void Step() override
{
Sample::Step();
b3HexColor color1 = b3_colorOrange;
b3HexColor color2 = b3_colorAqua;
b3HexColor green = b3_colorGreen;
b3HexColor gray = b3_colorGray;
b3CastResultFcn* functions[] = { RayCastAnyCallback, RayCastClosestCallback, RayCastMultipleCallback,
RayCastSortedCallback };
b3CastResultFcn* modeFcn = functions[m_mode];
m_castContext = {};
m_castContext.initialOverlap = m_initialOverlap;
// Must initialize fractions for sorting
m_castContext.fractions[0] = FLT_MAX;
m_castContext.fractions[1] = FLT_MAX;
m_castContext.fractions[2] = FLT_MAX;
b3Sphere sphere = { m_origin, m_castRadius };
b3Capsule capsule = { m_origin, m_origin + b3Vec3{ 0.0f, 1.0f, 0.0f }, m_castRadius };
b3BoxHull box = {};
b3Vec3 pointBuffer[2];
b3ShapeProxy proxy = {};
switch ( m_castType )
{
case e_rayCast:
proxy.count = 0;
break;
case e_sphereCast:
proxy.count = 1;
proxy.radius = m_castRadius;
proxy.points = &sphere.center;
break;
case e_capsuleCast:
proxy.count = 2;
proxy.radius = m_castRadius;
pointBuffer[0] = capsule.center1;
pointBuffer[1] = capsule.center2;
proxy.points = pointBuffer;
break;
case e_boxCast:
{
b3Vec3 extent = { m_castRadius, 0.5f * m_castRadius, 0.25f * m_castRadius };
box = b3MakeTransformedBoxHull( extent.x, extent.y, extent.z, { m_origin, b3Quat_identity } );
proxy.points = box.boxPoints;
proxy.count = box.base.vertexCount;
}
break;
default:
assert( false );
break;
}
if ( m_castType == e_rayCast )
{
b3World_CastRay( m_worldId, m_origin, m_translation, b3DefaultQueryFilter(), modeFcn, &m_castContext );
}
else
{
b3World_CastShape( m_worldId, &proxy, m_translation, b3DefaultQueryFilter(), modeFcn, &m_castContext );
}
if ( m_castContext.count > 0 )
{
assert( m_castContext.count <= 3 );
b3HexColor colors[3] = { b3_colorRed, b3_colorGreen, b3_colorBlue };
DrawLine( m_origin, m_origin + m_translation, MakeColor( color2 ) );
for ( int i = 0; i < m_castContext.count; ++i )
{
b3Vec3 point = m_castContext.points[i];
b3Vec3 normal = m_castContext.normals[i];
DrawPoint( point, 10.0f, MakeColor( colors[i] ) );
b3Vec3 head = point + 0.5f * normal;
b3Transform transform = { m_castContext.fractions[i] * m_translation, b3Quat_identity };
if ( m_castType == e_rayCast )
{
DrawLine( point, head, MakeColor( colors[i] ) );
}
else if ( m_castType == e_sphereCast )
{
DrawLine( point, head, MakeColor( color1 ) );
DrawSolidSphere( transform, sphere, MakeColorAlpha( colors[i], 0.5f ) );
}
else if ( m_castType == e_capsuleCast )
{
DrawLine( point, head, MakeColor( color1 ) );
DrawSolidCapsule( transform, capsule, MakeColor( colors[i] ) );
}
else if ( m_castType == e_boxCast )
{
DrawLine( point, head, MakeColor( color1 ) );
DrawHull( transform, &box.base, MakeColor( colors[i] ) );
}
}
}
else
{
DrawLine( m_origin, m_origin + m_translation, MakeColor( color2 ) );
b3Transform transform = { m_translation, b3Quat_identity };
if ( m_castType == e_sphereCast )
{
DrawSolidSphere( transform, sphere, MakeColor( gray ) );
}
else if ( m_castType == e_capsuleCast )
{
DrawSolidCapsule( transform, capsule, MakeColor( gray ) );
}
else if ( m_castType == e_boxCast )
{
DrawHull( transform, &box.base, MakeColor( gray ) );
}
}
DrawPoint( m_origin, 10.0f, MakeColor( green ) );
}
static constexpr int m_maxCount = 64;
static constexpr int m_ignoreBase = 0x3;
b3Capsule m_capsule;
b3Sphere m_sphere;
b3BoxHull m_box;
b3MeshData* m_mesh;
b3HeightField* m_heightField;
b3BodyId m_bodies[m_maxCount];
int m_bodyIndex;
int m_mode;
int m_castType;
float m_castRadius;
bool m_initialOverlap;
b3Vec3 m_origin;
b3Vec3 m_translation;
CastContext m_castContext;
};
static int sampleCastWorld = RegisterSample( "Collision", "Cast World", CastWorld::Create );
class MeshScale : public Sample
{
public:
static Sample* Create( SampleContext* context )
{
return new MeshScale( context );
}
explicit MeshScale( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 20.0f );
}
m_mesh = b3CreateBoxMesh( { 0.0f, 0.0f, 0.0f }, { 0.5f, 0.5f, 0.5f }, true );
m_scale = { 1.0f, 1.0f, 1.0f };
b3BodyDef bodyDef = b3DefaultBodyDef();
b3ShapeDef shapeDef = b3DefaultShapeDef();
m_meshBodyId = b3CreateBody( m_worldId, &bodyDef );
m_meshShapeId = b3CreateMeshShape( m_meshBodyId, &shapeDef, m_mesh, m_scale );
m_start = { -2.0f, 0.0f, 0.0f };
m_sphereCast = true;
}
~MeshScale() override
{
b3DestroyMesh( m_mesh );
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
b3Vec3 scale = m_scale;
bool changed = false;
changed = changed || ImGui::SliderFloat( "Scale X", &scale.x, -2.0f, 2.0f, "%.1f" );
changed = changed || ImGui::SliderFloat( "Scale Y", &scale.y, -2.0f, 2.0f, "%.1f" );
changed = changed || ImGui::SliderFloat( "Scale Z", &scale.z, -2.0f, 2.0f, "%.1f" );
if ( changed )
{
m_scale = scale;
b3Shape_SetMesh( m_meshShapeId, m_mesh, m_scale );
}
ImGui::SliderFloat( "Start Y", &m_start.y, -2.0f, 2.0f, "%.1f" );
ImGui::SliderFloat( "Start Z", &m_start.z, -2.0f, 2.0f, "%.1f" );
ImGui::Checkbox( "sphere Cast", &m_sphereCast );
return true;
}
void Step() override
{
b3Vec3 rayOrigin = m_start;
b3Vec3 rayTranslation = { 4.0f, 0.0f, 0.0f };
DrawPoint( rayOrigin, 8.0f, MakeColor( b3_colorGreen ) );
DrawPoint( rayOrigin + rayTranslation, 8.0f, MakeColor( b3_colorRed ) );
DrawLine( rayOrigin, rayOrigin + rayTranslation, MakeColor( b3_colorWhite ) );
if ( m_sphereCast )
{
b3Sphere sphere = { rayOrigin, 0.25f };
CastContext context = {};
b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius };
b3World_CastShape( m_worldId, &proxy, rayTranslation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
if ( context.count > 0 )
{
b3Transform transform = { context.fractions[0] * rayTranslation, b3Quat_identity };
DrawSolidSphere( transform, sphere, MakeColor( b3_colorYellow ) );
b3Vec3 point = context.points[0];
DrawLine( point, point + 0.5f * context.normals[0], MakeColor( b3_colorGreen ) );
DrawPoint( point, 5.0f, MakeColor( b3_colorYellow ) );
}
else
{
b3Transform transform = { rayTranslation, b3Quat_identity };
DrawSolidSphere( transform, sphere, MakeColor( b3_colorGray ) );
}
}
else
{
b3RayResult result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() );
if ( result.hit )
{
b3Vec3 point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGreen ) );
DrawPoint( point, 5.0f, MakeColor( b3_colorYellow ) );
}
}
}
b3Vec3 m_scale;
b3MeshData* m_mesh;
b3BodyId m_meshBodyId;
b3ShapeId m_meshShapeId;
b3Vec3 m_start;
bool m_sphereCast;
};
static int sampleMeshScale = RegisterSample( "Collision", "Mesh Scale", MeshScale::Create );
class ShapeCast : public Sample
{
public:
explicit ShapeCast( SampleContext* context )
: Sample( context )
{
m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } );
b3BoxHull box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f );
m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f );
b3BodyDef bodyDef = b3DefaultBodyDef();
b3ShapeDef shapeDef = b3DefaultShapeDef();
for ( int index = 0; index < 3; ++index )
{
bodyDef.position = { -6.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI );
b3BodyId sphereBody = b3CreateBody( m_worldId, &bodyDef );
b3Sphere sphere = { b3Vec3_zero, 0.9f };
b3CreateSphereShape( sphereBody, &shapeDef, &sphere );
bodyDef.position = { -2.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
b3BodyId capsuleBody = b3CreateBody( m_worldId, &bodyDef );
b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.7f };
b3CreateCapsuleShape( capsuleBody, &shapeDef, &capsule );
bodyDef.position = { 2.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
b3BodyId hullBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( hullBody, &shapeDef, &box.base );
bodyDef.position = { 6.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI );
b3BodyId meshBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateMeshShape( meshBody, &shapeDef, m_mesh, b3Vec3_one );
// todo add height field
}
m_baseX = 0;
m_baseY = 0;
m_castOffset = {};
m_trackingX = false;
m_trackingY = false;
m_initialOverlap = false;
}
~ShapeCast() override
{
b3DestroyMesh( m_mesh );
}
void CastSpheres() const
{
for ( int castIndex = 0; castIndex < 4; ++castIndex )
{
// World space sweep
b3Sphere sphere = { b3Vec3_zero, 0.3f };
b3Vec3 offset = b3Vec3{ -6.0f + 4.0f * castIndex, 3.0f, -5.0f } + m_castOffset;
sphere.center += offset;
b3Vec3 translation = 10.0f * b3Vec3_axisZ;
b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius };
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 point = context.points[0];
b3Vec3 normal = context.normals[0];
float fraction = context.fractions[0];
// final position with overlap resolution
DrawSolidSphere( { fraction * translation, b3Quat_identity }, sphere, MakeColor( b3_colorRed ) );
DrawPoint( point, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( point, point + 0.2f * normal, MakeColor( b3_colorYellow ) );
}
else
{
DrawSolidSphere( { translation, b3Quat_identity }, sphere, MakeColor( b3_colorGray ) );
}
}
}
void CastCapsules() const
{
for ( int castIndex = 0; castIndex < 4; ++castIndex )
{
b3Capsule capsule = { { -0.2f, -0.2f, -0.2f }, { 0.2f, 0.2f, 0.2f }, 0.2f };
b3Vec3 offset = b3Vec3{ -6.0f + 4.0f * castIndex, 5.0f, -5.0f } + m_castOffset;
capsule.center1 += offset;
capsule.center2 += offset;
b3Vec3 translation = 10.0f * b3Vec3_axisZ;
b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius };
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 point = context.points[0];
b3Vec3 normal = context.normals[0];
float fraction = context.fractions[0];
DrawSolidCapsule( { fraction * translation, b3Quat_identity }, capsule, MakeColor( b3_colorRed ) );
DrawPoint( point, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( point, point + 0.2f * normal, MakeColor( b3_colorYellow ) );
}
else
{
DrawSolidCapsule( { translation, b3Quat_identity }, capsule, MakeColor( b3_colorGray ) );
}
}
}
void CastHulls() const
{
for ( int castIndex = 0; castIndex < 4; ++castIndex )
{
b3Vec3 offset = b3Vec3{ -6.0f + 4.0f * castIndex, 7.0f, -5.0f } + m_castOffset;
b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.25f * B3_PI );
b3Quat qy = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.25f * B3_PI );
b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
b3Quat q = b3MulQuat( qx, b3MulQuat( qy, qz ) );
b3Transform transform = { offset, q };
b3BoxHull box = b3MakeTransformedBoxHull( 0.3f, 0.3f, 0.3f, transform );
b3Vec3 translation = 10.0f * b3Vec3_axisZ;
b3ShapeProxy proxy = { box.boxPoints, box.base.vertexCount, 0.0f };
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
DrawHull( b3Transform_identity, &box.base, MakeColor( b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 point = context.points[0];
b3Vec3 normal = context.normals[0];
float fraction = context.fractions[0];
// final position with overlap resolution
DrawHull( { fraction * translation, b3Quat_identity }, &box.base, MakeColor( b3_colorRed ) );
DrawPoint( point, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( point, point + 0.2f * normal, MakeColor( b3_colorYellow ) );
}
else
{
DrawHull( { translation, b3Quat_identity }, &box.base, MakeColor( b3_colorGray ) );
}
}
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
ImGui::Checkbox( "Initial Overlap", &m_initialOverlap );
return true;
}
void Render() override
{
Sample::Render();
DrawTextLine( "Shift + LMB and drag to shift start position" );
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
}
void MouseDown( b3Vec2 p, int button, int modifiers ) override
{
if ( button == 0 )
{
if ( modifiers == MOD_SHIFT )
{
m_trackingX = true;
m_baseX = p.x;
}
else if ( modifiers == MOD_CTRL )
{
m_trackingY = true;
m_baseY = p.y;
}
}
}
void MouseUp( b3Vec2 p, int button ) override
{
if ( button == 0 )
{
m_trackingX = false;
m_trackingY = false;
}
}
void MouseMove( b3Vec2 p ) override
{
if ( m_trackingX )
{
m_castOffset.z = 0.05f * ( m_baseX - p.x );
}
if ( m_trackingY )
{
m_castOffset.y = 0.05f * ( m_baseY - p.y );
}
}
void Step() override
{
CastSpheres();
CastCapsules();
CastHulls();
}
static Sample* Create( SampleContext* context )
{
return new ShapeCast( context );
}
b3MeshData* m_mesh;
int m_baseX;
int m_baseY;
b3Vec3 m_castOffset;
bool m_trackingX;
bool m_trackingY;
bool m_initialOverlap;
};
static int sampleShapeCast = RegisterSample( "Collision", "Shape Cast", ShapeCast::Create );
// Tests shape overlap versus world
class OverlapWorld : public Sample
{
public:
enum
{
BODY_COUNT = 12
};
static Sample* Create( SampleContext* context )
{
return new OverlapWorld( context );
}
explicit OverlapWorld( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } );
}
b3World_SetGravity( m_worldId, b3Vec3_zero );
m_box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f );
m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f );
m_heightField = b3CreateWave( 10, 10, 0.2f * b3Vec3_one, 0.03f, 0.09f, false );
b3BodyDef bodyDef = b3DefaultBodyDef();
b3ShapeDef shapeDef = b3DefaultShapeDef();
for ( int index = 0; index < 3; ++index )
{
b3BodyType type = b3BodyType( index );
bodyDef.type = type;
bodyDef.position = { -6.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI );
b3BodyId sphereBody = b3CreateBody( m_worldId, &bodyDef );
b3Sphere sphere = { b3Vec3_zero, 0.8f };
b3CreateSphereShape( sphereBody, &shapeDef, &sphere );
bodyDef.type = type;
bodyDef.position = { -3.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
b3BodyId capsuleBody = b3CreateBody( m_worldId, &bodyDef );
b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.5f };
b3CreateCapsuleShape( capsuleBody, &shapeDef, &capsule );
bodyDef.type = type;
bodyDef.position = { 0.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
b3BodyId hullBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( hullBody, &shapeDef, &m_box.base );
bodyDef.type = type;
bodyDef.position = { 3.0f, 3.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI );
b3BodyId meshBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateMeshShape( meshBody, &shapeDef, m_mesh, { -0.5f, 1.5f, -1.0f } );
// Height fields only on static bodies
bodyDef.type = b3_staticBody;
bodyDef.position = { 5.0f, 2.0f + 2.0f * index, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, -0.5f * B3_PI );
b3BodyId heightBody = b3CreateBody( m_worldId, &bodyDef );
b3CreateHeightFieldShape( heightBody, &shapeDef, m_heightField );
}
m_baseX = 0;
m_castOffset = 0.0f;
m_tracking = false;
}
~OverlapWorld() override
{
b3DestroyHeightField( m_heightField );
b3DestroyMesh( m_mesh );
}
static bool OverlapResultFcn( b3ShapeId shapeId, void* context )
{
b3HexColor* color = static_cast<b3HexColor*>( context );
*color = b3_colorRed;
// terminate the query
return false;
}
void OverlapSpheres() const
{
for ( int i = 0; i < 5; ++i )
{
b3Sphere sphere = { { -6.0f + 3.0f * i, 3.0f, -5.0f + m_castOffset }, 0.3f };
b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius };
b3HexColor color = b3_colorGreen;
b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( color ) );
}
}
void OverlapCapsules() const
{
for ( int i = 0; i < 5; ++i )
{
b3Vec3 offset = { -6.0f + 3.0f * i, 5.0f, -5.0f + m_castOffset };
b3Capsule capsule = { { -0.2f, -0.2f, -0.2f }, { 0.2f, 0.2f, 0.2f }, 0.2f };
capsule.center1 += offset;
capsule.center2 += offset;
b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius };
b3HexColor color = b3_colorGreen;
b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( color ) );
}
}
void OverlapHulls() const
{
for ( int i = 0; i < 5; ++i )
{
b3Transform transform{ { -6.0f + 3.0f * i, 7.0f, -5.0f + m_castOffset }, b3Quat_identity };
b3BoxHull box = b3MakeTransformedBoxHull( 0.3f, 0.3f, 0.3f, transform );
b3ShapeProxy proxy = { box.boxPoints, box.base.vertexCount, 0.0f };
b3HexColor color = b3_colorGreen;
b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawHull( b3Transform_identity, &box.base, MakeColor( color ) );
}
}
void Render() override
{
Sample::Render();
DrawTextLine( "Shift + LMB and drag to move shapes" );
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
}
void MouseDown( b3Vec2 p, int button, int modifiers ) override
{
if ( button == 0 )
{
if ( modifiers == MOD_SHIFT )
{
m_tracking = true;
m_baseX = p.x;
}
}
}
void MouseUp( b3Vec2 p, int button ) override
{
if ( button == 0 )
{
m_tracking = false;
}
}
void MouseMove( b3Vec2 p ) override
{
if ( m_tracking )
{
m_castOffset = 0.05f * ( m_baseX - p.x );
}
}
void Step() override
{
OverlapSpheres();
OverlapCapsules();
OverlapHulls();
}
b3BoxHull m_box;
b3MeshData* m_mesh;
b3HeightField* m_heightField;
int m_baseX;
float m_castOffset;
bool m_tracking;
};
static int sampleOverlapWorld = RegisterSample( "Collision", "Overlap World", OverlapWorld::Create );
class InitialOverlap : public Sample
{
public:
explicit InitialOverlap( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( -140.0f, 10.0f, 10.0f, { 0.0f, 0.0f, 0.0f } );
}
int indices[] = { 0, 1, 2, 2, 3, 0 };
b3Vec3 vertices[] = { { -0.5f, 0.5f, 0.5f }, { -0.5f, 0.5f, -0.5f }, { -0.5f, -0.5f, -0.5f }, { -0.5f, -0.5f, 0.5f } };
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = { 0.0f, 0.0f, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 10.0f * B3_DEG_TO_RAD );
b3BodyId body = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
// m_mesh = b3CreateBoxMesh( { 0.0f, 0.0f, 0.0f }, { 0.5f, 0.5f, 0.5f } );
b3MeshDef def = {};
def.triangleCount = 2;
def.indices = indices;
def.vertexCount = 4;
def.vertices = vertices;
def.materialIndices = nullptr;
def.useMedianSplit = false;
m_mesh = b3CreateMesh( &def, nullptr, 0 );
b3CreateMeshShape( body, &shapeDef, m_mesh, 4.0f * b3Vec3_one );
m_initialOverlap = true;
}
~InitialOverlap() override
{
b3DestroyMesh( m_mesh );
}
void Render() override
{
Sample::Render();
DrawAxes( b3Transform_identity, 1.0f );
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
ImGui::Checkbox( "initial overlap", &m_initialOverlap );
return true;
}
void Step() override
{
Sample::Step();
b3Vec3 offset = { -2.1f, -0.8f, 0.95f };
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3Capsule capsule = {
.center1 = offset,
.center2 = offset + b3Vec3{ 0.0f, 1.0f, 0.0f },
.radius = 0.25f,
};
// zero length cast
b3Vec3 translation = { 0.0f, 0.0f, 0.0f };
b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius };
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorGreen ) );
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
float fraction = context.count > 0 ? context.fractions[0] : 1.0f;
b3Transform shapeEnd = { fraction * translation, b3Quat_identity };
DrawSolidCapsule( shapeEnd, capsule, MakeColor( context.count > 0 ? b3_colorRed : b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 p = context.points[0];
b3Vec3 n = context.normals[0];
DrawLine( p, p + 0.5f * n, MakeColor( b3_colorAliceBlue ) );
DrawPoint( p, 8.0f, MakeColor( b3_colorAliceBlue ) );
}
}
static Sample* Create( SampleContext* context )
{
return new InitialOverlap( context );
}
b3MeshData* m_mesh;
bool m_initialOverlap;
};
static int sampleInitialOverlap = RegisterSample( "Collision", "Initial Overlap", InitialOverlap::Create );
// pointA[0] = {0.000000000, 0.000000000, 0.000000000}
// pointA[1] = {0.000000000, -6400.000000000, 0.000000000}
// pointA[2] = {6400.000000000, 0.000000000, 22.609375000}
// pointB[i] = {43616.210937500, -100213.000000000, 132631.812500000}
// pointB[i] = {342231.968750000, 359711.687500000, 132631.812500000}
// radiusB = 40.0f
// xfA = {{0.000000000, 0.000000000, 0.000000000}, {{0.000000000, 0.000000000, 0.000000000}, 1.000000000}
// xfB = {{-115200.000000000, -19200.000000000, -202755.000000000}, {{0.000000000, 0.000000000, 0.000000000}, 1.000000000}
// t = {0.008614914, 0.000000000, 72267.117187500}
// maxFraction = 0.970617533, canEncroach = 0
class ShapeCastDebug : public Sample
{
public:
explicit ShapeCastDebug( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } );
}
// this triggers an assert with scale of 1 and b3_lengthUnitsPerMeter of 100
float scale = 0.01f;
m_triangle[0] = scale * b3Vec3{ 0.000000000, 0.000000000, 0.000000000 };
m_triangle[1] = scale * b3Vec3{ 0.000000000, -6400.000000000, 0.000000000 };
m_triangle[2] = scale * b3Vec3{ 6400.000000000, 0.000000000, 22.609375000 };
b3Vec3 origin = m_triangle[0];
m_triangle[0] = b3Vec3_zero;
m_triangle[1] -= origin;
m_triangle[2] -= origin;
m_points[0] = scale * b3Vec3{ 200.305283, 200.460999, 9.53760529 };
m_points[1] = scale * b3Vec3{ -200.305283, 200.460999, 9.53760529 };
m_points[2] = scale * b3Vec3{ -200.305283, -200.460999, 9.53760529 };
m_points[3] = scale * b3Vec3{ 200.305283, -200.460999, 9.53760529 };
m_points[4] = scale * b3Vec3{ 200.305283, 200.460999, -9.53760529 };
m_points[5] = scale * b3Vec3{ -200.305283, 200.460999, -9.53760529 };
m_points[6] = scale * b3Vec3{ -200.305283, -200.460999, -9.53760529 };
m_points[7] = scale * b3Vec3{ 200.305283, -200.460999, -9.53760529 };
m_capsule.center1 = scale * b3Vec3{ 43616.210937500, -100213.000000000, 132631.812500000 };
m_capsule.center2 = scale * b3Vec3{ 342231.968750000, 359711.687500000, 132631.812500000 };
m_capsule.radius = scale * 1.0f;
float length = b3Distance( m_capsule.center1, m_capsule.center2 );
(void)length;
m_transform = { scale * b3Vec3{ -115200.000000000, -19200.000000000, -202755.000000000 } - origin,
{ { 0.00000000, 0.00000000, 0.0f }, 1.0f } };
m_translation = scale * b3Vec3{ 0.008614914, 0.000000000, 72267.117187500 };
m_box = b3CreateHull( m_points, 8, 8 );
}
~ShapeCastDebug() override
{
b3DestroyHull( m_box );
}
void Render() override
{
Sample::Render();
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
b3ShapeCastPairInput input;
input.proxyA = { m_triangle, 3, 0.0f };
// input.proxyB = { m_points, 8, 0.0f };
input.proxyB = { &m_capsule.center1, 2, m_capsule.radius };
input.transformA = b3Transform_identity;
input.transformB = m_transform;
input.translationB = m_translation;
input.maxFraction = 0.970617533;
input.canEncroach = false;
b3CastOutput output = b3ShapeCast( &input );
DrawTriangle( m_triangle[0], m_triangle[1], m_triangle[2], MakeColor( b3_colorCyan ) );
// DrawHull( m_scene, m_transform, m_box, b3_colorGreen, false );
DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorGreen ) );
if ( output.hit )
{
// final position with overlap resolution
b3Vec3 shapeEnd = m_transform.p + output.fraction * m_translation;
// DrawHull( m_scene, { shapeEnd, m_transform.q }, m_box, b3_colorRed, false );
DrawSolidCapsule( { shapeEnd, m_transform.q }, m_capsule, MakeColor( b3_colorRed ) );
}
b3Vec3 shapeEnd = m_transform.p + m_translation;
// DrawHull( m_scene, { shapeEnd, m_transform.q }, m_box, b3_colorGray, false );
DrawSolidCapsule( { shapeEnd, m_transform.q }, m_capsule, MakeColor( b3_colorGray ) );
}
static Sample* Create( SampleContext* context )
{
return new ShapeCastDebug( context );
}
b3Transform m_transform;
b3Vec3 m_translation;
b3Capsule m_capsule;
b3HullData* m_box;
b3Vec3 m_points[8];
b3Vec3 m_triangle[3];
};
static int sampleShapeCastDebug = RegisterSample( "Collision", "Shape Cast Debug", ShapeCastDebug::Create );
class DistanceDebug : public Sample
{
public:
explicit DistanceDebug( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } );
}
// this triggers an assert with scale of 1 and b3_lengthUnitsPerMeter of 100
float scale = 0.01f;
m_triangle[0] = scale * b3Vec3{ 1400.00000, 1600.00000, 70.1534424 };
m_triangle[1] = scale * b3Vec3{ 1400.00000, 1500.00000, 66.1250000 };
m_triangle[2] = scale * b3Vec3{ 1500.00000, 1600.00000, 72.3507080 };
// shifting to the origin give more accuracy
b3Vec3 origin = m_triangle[0];
m_triangle[0] = b3Vec3_zero;
m_triangle[1] -= origin;
m_triangle[2] -= origin;
m_points[0] = scale * b3Vec3{ 200.305283, 200.460999, 9.53760529 };
m_points[1] = scale * b3Vec3{ -200.305283, 200.460999, 9.53760529 };
m_points[2] = scale * b3Vec3{ -200.305283, -200.460999, 9.53760529 };
m_points[3] = scale * b3Vec3{ 200.305283, -200.460999, 9.53760529 };
m_points[4] = scale * b3Vec3{ 200.305283, 200.460999, -9.53760529 };
m_points[5] = scale * b3Vec3{ -200.305283, 200.460999, -9.53760529 };
m_points[6] = scale * b3Vec3{ -200.305283, -200.460999, -9.53760529 };
m_points[7] = scale * b3Vec3{ 200.305283, -200.460999, -9.53760529 };
m_boxA = b3MakeBoxHull( 40.0f, 1.0f, 40.0f );
m_boxB = b3MakeTransformedBoxHull( 0.5f, 10.0f, 0.5f, { { 0.0f, 10.0f, 0.0f }, b3Quat_identity } );
// m_transformA = {
// .p = { 0.0f, -1.0f, 0.0f },
// .q = b3Quat_identity,
// };
m_transformB = {
.p = { -1.64657831e-06, 1.00989532471, 0.0f },
.q = { { 0.00000000, 0.00000000, 0.00494779600 }, 0.999987781 },
};
m_box = b3CreateHull( m_points, 8, 8 );
m_simplexCount = 0;
m_simplexIndex = 0;
memset( m_simplexes, 0, sizeof( m_simplexes ) );
}
~DistanceDebug() override
{
b3DestroyHull( m_box );
}
void Render() override
{
Sample::Render();
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
ImGui::SliderInt( "simplex index", &m_simplexIndex, 0, m_simplexCount - 1 );
return true;
}
static void BuildWitnessPoints( const b3Simplex* simplex, b3Vec3& vertexA, b3Vec3& vertexB )
{
const b3SimplexVertex* vs = simplex->vertices;
int count = simplex->count;
B3_ASSERT( 1 <= count && count <= 4 );
switch ( count )
{
case 1:
vertexA = vs[0].wA;
vertexB = vs[0].wB;
break;
case 2:
vertexA = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA;
vertexB = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB;
break;
case 3:
vertexA = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA;
vertexB = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB + vs[2].a * vs[2].wB;
break;
case 4:
// Force identical points and *zero* distance
vertexA = vertexB = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA + vs[3].a * vs[3].wA;
break;
default:
B3_ASSERT( !"Should never get here!" );
break;
}
}
static b3Vec3 GetClosestPoint( const b3Simplex* simplex )
{
int count = simplex->count;
B3_ASSERT( 1 <= count && count <= 4 );
const b3SimplexVertex* vs = simplex->vertices;
switch ( count )
{
case 1:
return simplex->vertices[0].w;
case 2:
return vs[0].a * vs[0].w + vs[1].a * vs[1].w;
case 3:
return vs[0].a * vs[0].w + vs[1].a * vs[1].w + vs[2].a * vs[2].w;
case 4:
return vs[0].a * vs[0].w + vs[1].a * vs[1].w + vs[2].a * vs[2].w + vs[3].a * vs[3].w;
default:
B3_ASSERT( !"Should never get here!" );
break;
}
return b3Vec3_zero;
}
void Step() override
{
b3DistanceInput input;
input.proxyA = { m_boxA.boxPoints, 8, 0.0f };
input.proxyB = { m_boxB.boxPoints, 8, 0.0f };
input.transformA = b3Transform_identity;
input.transformB = m_transformB;
input.useRadii = false;
b3SimplexCache cache = {};
b3DistanceOutput output = b3ShapeDistance( &input, &cache, m_simplexes, 32 );
m_simplexCount = output.simplexCount;
// DrawFace( m_scene, m_triangle[0], m_triangle[1], m_triangle[2], b3_colorCyan );
// DrawHull( m_scene, m_transformB, m_box, b3_colorGreen, false );
DrawHull( b3Transform_identity, &m_boxA.base, MakeColor( b3_colorGreen ) );
DrawHull( m_transformB, &m_boxB.base, MakeColor( b3_colorCyan ) );
for ( int i = 0; i < m_boxB.base.vertexCount; ++i )
{
b3Vec3 p = b3TransformPoint( m_transformB, m_boxB.boxPoints[i] );
DrawWorldString( p, MakeColor( b3_colorAliceBlue ), " %d", i );
}
DrawPoint( output.pointA, 5.0f, MakeColor( b3_colorWhite ) );
DrawPoint( output.pointB, 5.0f, MakeColor( b3_colorWhite ) );
DrawLine( output.pointA, output.pointA + 1.0f * output.normal, MakeColor( b3_colorWhite ) );
b3Vec3 n = output.normal;
DrawTextLine( "distance = %g, normal = %g, %g, %g", output.distance, n.x, n.y, n.z );
B3_ASSERT( output.distance > 0.0f && b3IsNormalized( output.normal ) );
if ( m_simplexCount > 0 )
{
b3Simplex* simplex = m_simplexes + m_simplexIndex;
b3Vec3 v1, v2;
BuildWitnessPoints( simplex, v1, v2 );
DrawPoint( v1, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( v2, 10.0f, MakeColor( b3_colorGreen ) );
for ( int i = 0; i < simplex->count; ++i )
{
v1 = simplex->vertices[i].wA;
v2 = simplex->vertices[i].wB;
DrawPoint( v1, 5.0f, MakeColor( b3_colorRed ) );
DrawPoint( v2, 5.0f, MakeColor( b3_colorRed ) );
}
b3Vec3 p = GetClosestPoint( simplex );
float distance = b3Length( p );
DrawTextLine( "current distance = %.5f", distance );
}
for ( int i = 0; i < m_simplexCount; ++i )
{
b3Simplex* simplex = m_simplexes + i;
if ( simplex->count == 1 )
{
b3SimplexVertex* v1 = simplex->vertices + 0;
DrawTextLine( "v1s : %d, v2s : %d", v1->indexA, v1->indexB );
}
else if ( simplex->count == 2 )
{
b3SimplexVertex* v1 = simplex->vertices + 0;
b3SimplexVertex* v2 = simplex->vertices + 1;
DrawTextLine( "v1s : %d/%d, v2s : %d/%d", v1->indexA, v2->indexA, v1->indexB, v2->indexB );
}
else if ( simplex->count == 3 )
{
b3SimplexVertex* v1 = simplex->vertices + 0;
b3SimplexVertex* v2 = simplex->vertices + 1;
b3SimplexVertex* v3 = simplex->vertices + 2;
DrawTextLine( "v1s : %d/%d/%d, v2s : %d/%d/%d", v1->indexA, v2->indexA, v3->indexA, v1->indexB, v2->indexB,
v3->indexB );
}
else if ( simplex->count == 3 )
{
b3SimplexVertex* v1 = simplex->vertices + 0;
b3SimplexVertex* v2 = simplex->vertices + 1;
b3SimplexVertex* v3 = simplex->vertices + 2;
b3SimplexVertex* v4 = simplex->vertices + 3;
DrawTextLine( "v1s : %d/%d/%d/%d, v2s : %d/%d/%d/%d", v1->indexA, v2->indexA, v3->indexA, v4->indexA, v1->indexB,
v2->indexB, v3->indexB, v4->indexB );
}
}
}
static Sample* Create( SampleContext* context )
{
return new DistanceDebug( context );
}
b3Transform m_transformB;
b3HullData* m_box;
b3BoxHull m_boxA;
b3BoxHull m_boxB;
b3Vec3 m_points[8];
b3Vec3 m_triangle[3];
b3Simplex m_simplexes[32];
int m_simplexIndex;
int m_simplexCount;
};
static int sampleDistanceDebug = RegisterSample( "Collision", "Distance Debug", DistanceDebug::Create );
class ShapeDistance : public Sample
{
public:
enum ShapeType
{
e_point,
e_segment,
e_triangle,
e_box
};
explicit ShapeDistance( SampleContext* context )
: Sample( context )
{
if ( m_context->restart == false )
{
m_camera->SetView( -90.0f, 0.0f, 10.0f, { 0.0f, 0.0f, 0.0f } );
}
m_point = b3Vec3_zero;
m_segment[0] = { -0.5f, 0.0f };
m_segment[1] = { 0.5f, 0.0f };
m_triangle[0] = { -0.5f, 0.0f };
m_triangle[1] = { 0.5f, 0.0f };
m_triangle[2] = { 0.0f, 1.0f };
m_box = b3MakeBoxHull( 0.25f, 0.5f, 1.0f );
m_triangle[0] = { 0.0f, 0.0f, -4.0f };
m_triangle[1] = { 0.0f, 0.0f, -8.0f };
m_triangle[2] = { -4.0f, 0.0f, -8.0f };
m_box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f );
m_transform = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity };
m_transform = {
.p = { 0.00000000, 0.796873450, -4.50000000 },
.q = { { -0.964603841, -0.0856034234, 0.248849452 }, 0.0168983955 },
};
m_cache = {};
m_simplexCount = 0;
m_simplexIndex = 0;
m_dragStart = { 0.0f, 0.0f };
m_basePosition = { 0.0f, 0.0f, 0.0f };
m_baseQuat = { .v = { .z = 1.0f } };
m_dragging = false;
m_rotating = false;
m_showIndices = false;
m_useCache = false;
m_drawSimplex = false;
m_typeA = e_triangle;
m_typeB = e_box;
m_radiusA = 0.0f;
m_radiusB = 0.0f;
m_proxyA = MakeProxy( m_typeA, m_radiusA );
m_proxyB = MakeProxy( m_typeB, m_radiusB );
}
b3ShapeProxy MakeProxy( ShapeType type, float radius )
{
b3ShapeProxy proxy = {};
proxy.radius = radius;
switch ( type )
{
case e_point:
proxy.points = &m_point;
proxy.count = 1;
break;
case e_segment:
proxy.points = m_segment;
proxy.count = 2;
break;
case e_triangle:
proxy.points = m_triangle;
proxy.count = 3;
break;
case e_box:
proxy.points = b3GetHullPoints( &m_box.base );
proxy.count = m_box.base.vertexCount;
break;
default:
assert( false );
}
return proxy;
}
void DrawShape( ShapeType type, b3Transform transform, float radius, b3HexColor color )
{
switch ( type )
{
case e_point:
{
if ( radius > 0.0f )
{
b3Sphere sphere = {
.center = m_point,
.radius = radius,
};
DrawSolidSphere( transform, sphere, MakeColor( color ) );
}
else
{
b3Vec3 p = b3TransformPoint( transform, m_point );
DrawPoint( p, 5.0f, MakeColor( color ) );
}
}
break;
case e_segment:
{
if ( radius > 0.0f )
{
b3Capsule capsule = {
.center1 = m_segment[0],
.center2 = m_segment[1],
.radius = radius,
};
DrawSolidCapsule( transform, capsule, MakeColor( color ) );
}
else
{
b3Vec3 p1 = b3TransformPoint( transform, m_segment[0] );
b3Vec3 p2 = b3TransformPoint( transform, m_segment[1] );
DrawLine( p1, p2, MakeColor( color ) );
}
}
break;
case e_triangle:
{
b3Vec3 p1 = b3TransformPoint( transform, m_triangle[0] );
b3Vec3 p2 = b3TransformPoint( transform, m_triangle[1] );
b3Vec3 p3 = b3TransformPoint( transform, m_triangle[2] );
DrawLine( p1, p2, MakeColor( color ) );
DrawLine( p2, p3, MakeColor( color ) );
DrawLine( p3, p1, MakeColor( color ) );
}
break;
case e_box:
DrawHull( transform, &m_box.base, MakeColor( color ) );
break;
default:
assert( false );
}
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
const char* shapeTypes[] = { "point", "segment", "triangle", "box" };
int shapeType = int( m_typeA );
if ( ImGui::Combo( "shape A", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) )
{
m_typeA = ShapeType( shapeType );
m_proxyA = MakeProxy( m_typeA, m_radiusA );
}
if ( ( m_typeA == e_point || m_typeA == e_segment ) && ImGui::SliderFloat( "radius A", &m_radiusA, 0.0f, 0.5f, "%.2f" ) )
{
m_proxyA.radius = m_radiusA;
}
shapeType = int( m_typeB );
if ( ImGui::Combo( "shape B", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) )
{
m_typeB = ShapeType( shapeType );
m_proxyB = MakeProxy( m_typeB, m_radiusB );
}
if ( ( m_typeB == e_point || m_typeB == e_segment ) && ImGui::SliderFloat( "radius B", &m_radiusB, 0.0f, 0.5f, "%.2f" ) )
{
m_proxyB.radius = m_radiusB;
}
ImGui::Separator();
ImGui::Checkbox( "show indices", &m_showIndices );
ImGui::Checkbox( "use cache", &m_useCache );
ImGui::Separator();
if ( ImGui::Checkbox( "draw simplex", &m_drawSimplex ) )
{
m_simplexIndex = 0;
}
if ( m_drawSimplex && m_simplexCount > 0 )
{
ImGui::SliderInt( "index", &m_simplexIndex, 0, m_simplexCount - 1 );
m_simplexIndex = b3ClampInt( m_simplexIndex, 0, m_simplexCount - 1 );
}
return true;
}
void MouseDown( b3Vec2 ps, int button, int mods ) override
{
if ( button == MOUSE_LEFT )
{
if ( mods == 0 && m_rotating == false )
{
m_dragging = true;
PickRay ray = m_camera->BuildPickRay( ps.x, ps.y );
b3Vec3 d = b3Normalize( ray.translation );
// Intersect with plane going through origin
// p = c + alpha * f
// dot(p, f) = 0
// dot(c, f) + alpha = 0
// p = c - dot(c, f) * f
m_dragStart = b3MulSub( ray.origin, b3Dot( ray.origin, d ), d );
m_basePosition = m_transform.p;
}
else if ( mods == MOD_SHIFT && m_dragging == false )
{
m_rotating = true;
m_rotateStart = ps.x;
m_baseQuat = m_transform.q;
}
}
}
void MouseUp( b3Vec2, int button ) override
{
if ( button == MOUSE_LEFT )
{
m_dragging = false;
m_rotating = false;
}
}
void MouseMove( b3Vec2 ps ) override
{
if ( m_dragging )
{
PickRay ray = m_camera->BuildPickRay( ps.x, ps.y );
b3Vec3 d = b3Normalize( ray.translation );
b3Vec3 p = b3MulSub( ray.origin, b3Dot( ray.origin, d ), d );
m_transform.p = m_basePosition + ( p - m_dragStart );
}
else if ( m_rotating && m_camera->m_width > 0.0f )
{
float dx = ( ps.x - m_rotateStart ) / m_camera->m_width;
float angle = b3ClampFloat( 2.0f * dx, -B3_PI, B3_PI );
b3Vec3 axis = m_camera->GetForward();
b3Quat deltaQuat = b3MakeQuatFromAxisAngle( axis, angle );
m_transform.q = b3MulQuat( deltaQuat, m_baseQuat );
}
}
static b3Vec3 Weight2( float a1, b3Vec3 w1, float a2, b3Vec3 w2 )
{
return { a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y };
}
static b3Vec3 Weight3( float a1, b3Vec3 w1, float a2, b3Vec3 w2, float a3, b3Vec3 w3 )
{
return { a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y };
}
static void ComputeWitnessPoints( b3Vec3* a, b3Vec3* b, const b3Simplex* s )
{
const b3SimplexVertex* vs = s->vertices;
int count = s->count;
B3_ASSERT( 1 <= count && count <= 4 );
switch ( count )
{
case 1:
*a = vs[0].wA;
*b = vs[0].wB;
break;
case 2:
*a = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA;
*b = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB;
break;
case 3:
*a = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA;
*b = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB + vs[2].a * vs[2].wB;
break;
case 4:
// Force identical points and *zero* distance
*a = *b = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA + vs[3].a * vs[3].wA;
break;
default:
B3_ASSERT( !"Should never get here!" );
break;
}
}
void Render() override
{
DrawAxes( b3Transform_identity, 0.5f );
Sample::Render();
}
void Step() override
{
b3DistanceInput input;
input.proxyA = m_proxyA;
input.proxyB = m_proxyB;
input.transformA = b3Transform_identity;
input.transformB = m_transform;
input.useRadii = m_radiusA > 0.0f || m_radiusB > 0.0f;
if ( m_useCache == false )
{
m_cache.count = 0;
}
b3DistanceOutput output = b3ShapeDistance( &input, &m_cache, m_simplexes, m_simplexCapacity );
if ( output.distance > 0.0f )
{
assert( b3IsNormalized( output.normal ) );
b3Vec3 d = b3Sub( output.pointB, output.pointA );
float length = b3Length( d );
assert( b3AbsFloat( length - output.distance ) < 10.0f * FLT_EPSILON );
b3Vec3 n = b3Normalize( d );
assert( b3AbsFloat( n.x - output.normal.x ) < 100.0f * FLT_EPSILON );
assert( b3AbsFloat( n.y - output.normal.y ) < 100.0f * FLT_EPSILON );
assert( b3AbsFloat( n.z - output.normal.z ) < 100.0f * FLT_EPSILON );
}
m_simplexCount = output.simplexCount;
DrawShape( m_typeA, b3Transform_identity, m_radiusA, b3_colorCyan );
DrawShape( m_typeB, m_transform, m_radiusB, b3_colorBisque );
if ( m_drawSimplex )
{
b3Simplex* simplex = m_simplexes + m_simplexIndex;
b3SimplexVertex* vertices = simplex->vertices;
if ( m_simplexIndex > 0 )
{
// The first recorded simplex does not have valid barycentric coordinates
b3Vec3 pointA, pointB;
ComputeWitnessPoints( &pointA, &pointB, simplex );
DrawLine( pointA, pointB, MakeColor( b3_colorWhite ) );
DrawPoint( pointA, 10.0f, MakeColor( b3_colorLightGreen ) );
DrawPoint( pointB, 10.0f, MakeColor( b3_colorLightBlue ) );
}
b3HexColor colors[3] = { b3_colorRed, b3_colorGreen, b3_colorBlue };
for ( int i = 0; i < simplex->count; ++i )
{
b3SimplexVertex* vertex = vertices + i;
DrawPoint( vertex->wA, 10.0f, MakeColor( colors[i] ) );
DrawPoint( vertex->wB, 10.0f, MakeColor( colors[i] ) );
}
}
else
{
DrawLine( output.pointA, output.pointB, MakeColor( b3_colorDimGray ) );
DrawPoint( output.pointA, 10.0f, MakeColor( b3_colorLightGreen ) );
DrawPoint( output.pointB, 10.0f, MakeColor( b3_colorLightBlue ) );
DrawLine( output.pointA, output.pointA + 0.5f * output.normal, MakeColor( b3_colorYellow ) );
}
if ( m_showIndices )
{
for ( int i = 0; i < m_proxyA.count; ++i )
{
b3Vec3 p = m_proxyA.points[i];
DrawWorldString( p, MakeColor( b3_colorWhite ), " %d", i );
}
for ( int i = 0; i < m_proxyB.count; ++i )
{
b3Vec3 p = b3TransformPoint( m_transform, m_proxyB.points[i] );
DrawWorldString( p, MakeColor( b3_colorWhite ), " %d", i );
}
}
DrawTextLine( "mouse button 1: drag" );
DrawTextLine( "mouse button 1 + shift: rotate" );
DrawTextLine( "distance = %.4f, iterations = %d", output.distance, output.iterations );
if ( m_cache.count == 1 )
{
DrawTextLine( "cache = {%d}, {%d}", m_cache.indexA[0], m_cache.indexB[0] );
}
else if ( m_cache.count == 2 )
{
DrawTextLine( "cache = {%d, %d}, {%d, %d}", m_cache.indexA[0], m_cache.indexA[1], m_cache.indexB[0],
m_cache.indexB[1] );
}
else if ( m_cache.count == 3 )
{
DrawTextLine( "cache = {%d, %d, %d}, {%d, %d, %d}", m_cache.indexA[0], m_cache.indexA[1], m_cache.indexA[2],
m_cache.indexB[0], m_cache.indexB[1], m_cache.indexB[2] );
}
}
static Sample* Create( SampleContext* context )
{
return new ShapeDistance( context );
}
static constexpr int m_simplexCapacity = 20;
b3BoxHull m_box;
b3Vec3 m_triangle[3];
b3Vec3 m_point;
b3Vec3 m_segment[2];
ShapeType m_typeA;
ShapeType m_typeB;
float m_radiusA;
float m_radiusB;
b3ShapeProxy m_proxyA;
b3ShapeProxy m_proxyB;
b3SimplexCache m_cache;
b3Simplex m_simplexes[m_simplexCapacity];
int m_simplexCount;
int m_simplexIndex;
b3Transform m_transform;
b3Vec3 m_basePosition;
b3Vec3 m_dragStart;
b3Quat m_baseQuat;
float m_rotateStart;
bool m_dragging;
bool m_rotating;
bool m_showIndices;
bool m_useCache;
bool m_drawSimplex;
};
static int sampleShapeDistance = RegisterSample( "Collision", "Shape Distance", ShapeDistance::Create );
class TimeOfImpact : public Sample
{
public:
enum ShapeType
{
e_box,
e_capsule,
e_triangle,
};
explicit TimeOfImpact( SampleContext* context )
: Sample( context )
{
if ( m_context->restart == false )
{
m_camera->SetView( -90.0f, 0.0f, 10.0f, { 0.0f, 0.0f, 0.0f } );
}
m_box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f );
m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.02f };
m_triangle[0] = { -4.0f, 0.0f, -4.0f };
m_triangle[1] = { -4.0f, 0.0f, -8.0f };
m_triangle[2] = { -8.0f, 0.0f, -8.0f };
m_typeA = e_triangle;
m_typeB = e_capsule;
m_proxyA = MakeProxy( m_typeA );
m_proxyB = MakeProxy( m_typeB );
m_sweepA = {};
m_sweepA.q1.s = 1.0f;
m_sweepA.q2.s = 1.0f;
m_sweepB = {};
m_sweepB.c1 = { -4.06512070, 0.101333618, -7.87591267 };
m_sweepB.c2 = { -4.15895557, 0.0356027633, -7.69682646 };
m_sweepB.q1 = { { -0.860495985, -0.272824734, 0.0724888667 }, 0.424097389 };
m_sweepB.q2 = { { -0.604184389, -0.424355596, 0.0457959622 }, 0.672894001 };
}
b3ShapeProxy MakeProxy( ShapeType type )
{
b3ShapeProxy proxy = {};
switch ( type )
{
case e_capsule:
proxy.points = &m_capsule.center1;
proxy.radius = m_capsule.radius;
proxy.count = 2;
break;
case e_triangle:
proxy.points = m_triangle;
proxy.count = 3;
break;
case e_box:
proxy.points = b3GetHullPoints( &m_box.base );
proxy.count = m_box.base.vertexCount;
break;
default:
assert( false );
}
return proxy;
}
void DrawShape( ShapeType type, b3Transform transform, b3HexColor color )
{
switch ( type )
{
case e_capsule:
{
DrawSolidCapsule( transform, m_capsule, MakeColor( color ) );
b3Vec3 center = b3Lerp( m_capsule.center1, m_capsule.center2, 0.5f );
center = b3TransformPoint( transform, center );
b3Transform xf = {
.p = center,
.q = transform.q,
};
DrawAxes( xf, 0.025f );
}
break;
case e_triangle:
{
b3Vec3 p1 = b3TransformPoint( transform, m_triangle[0] );
b3Vec3 p2 = b3TransformPoint( transform, m_triangle[1] );
b3Vec3 p3 = b3TransformPoint( transform, m_triangle[2] );
DrawLine( p1, p2, MakeColor( color ) );
DrawLine( p2, p3, MakeColor( color ) );
DrawLine( p3, p1, MakeColor( color ) );
DrawWorldString( p1, MakeColor( b3_colorWhite ), "0" );
DrawWorldString( p2, MakeColor( b3_colorWhite ), "1" );
DrawWorldString( p3, MakeColor( b3_colorWhite ), "2" );
}
break;
case e_box:
DrawHull( transform, &m_box.base, MakeColor( color ) );
break;
default:
assert( false );
}
}
bool HasSolverControls() const override
{
return false;
}
bool DrawControls() override
{
const char* shapeTypes[] = { "point", "segment", "triangle", "box" };
int shapeType = int( m_typeA );
if ( ImGui::Combo( "shape A", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) )
{
m_typeA = ShapeType( shapeType );
m_proxyA = MakeProxy( m_typeA );
}
shapeType = int( m_typeB );
if ( ImGui::Combo( "shape B", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) )
{
m_typeB = ShapeType( shapeType );
m_proxyB = MakeProxy( m_typeB );
}
return true;
}
void Render() override
{
DrawAxes( b3Transform_identity, 0.5f );
Sample::Render();
}
void Step() override
{
b3TOIInput input = {};
input.proxyA = m_proxyA;
input.proxyB = m_proxyB;
input.sweepA = m_sweepA;
input.sweepB = m_sweepB;
input.maxFraction = 1.0f;
b3TOIOutput output = b3TimeOfImpact( &input );
DrawShape( m_typeA, b3Transform_identity, b3_colorCyan );
b3Transform transform1 = b3GetSweepTransform( &m_sweepB, 0.0f );
b3Transform transform2 = b3GetSweepTransform( &m_sweepB, 1.0f );
// qr = inv(q1) * q2
b3Quat qr = b3InvMulQuat( transform1.q, transform2.q );
float angle;
(void)b3GetAxisAngle( &angle, qr );
DrawTextLine( "angle = %g", 180.0f * angle / B3_PI );
DrawShape( m_typeB, transform1, b3_colorLightGreen );
DrawShape( m_typeB, transform2, b3_colorLightCoral );
if ( output.fraction < 1.0f )
{
b3Transform transformHit = b3GetSweepTransform( &m_sweepB, output.fraction );
DrawShape( m_typeB, transformHit, b3_colorLightCyan );
}
if ( output.state == b3_toiStateHit || output.state == b3_toiStateFailed )
{
DrawLine( output.point, output.point + 0.5f * output.normal, MakeColor( b3_colorDimGray ) );
DrawPoint( output.point, 10.0f, MakeColor( b3_colorLightGreen ) );
}
switch ( output.state )
{
case b3_toiStateUnknown:
DrawTextLine( "unknown" );
break;
case b3_toiStateFailed:
DrawTextLine( "failed" );
break;
case b3_toiStateOverlapped:
DrawTextLine( "overlapped" );
break;
case b3_toiStateHit:
DrawTextLine( "hit %g", output.fraction );
break;
case b3_toiStateSeparated:
DrawTextLine( "separated" );
break;
default:
assert( false );
break;
}
DrawTextLine( "iterations / push / root = %d / %d / %d", output.distanceIterations, output.pushBackIterations,
output.rootIterations );
}
static Sample* Create( SampleContext* context )
{
return new TimeOfImpact( context );
}
b3BoxHull m_box;
b3Vec3 m_triangle[3];
b3Capsule m_capsule;
ShapeType m_typeA;
ShapeType m_typeB;
b3ShapeProxy m_proxyA;
b3ShapeProxy m_proxyB;
b3Sweep m_sweepA;
b3Sweep m_sweepB;
};
static int sampleTimeOfImpact = RegisterSample( "Collision", "Time of Impact", TimeOfImpact::Create );
class CapsuleCastRay : public Sample
{
public:
static Sample* Create( SampleContext* context )
{
return new CapsuleCastRay( context );
}
explicit CapsuleCastRay( SampleContext* context )
: Sample( context )
{
if ( m_context->restart == false )
{
m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_kinematicBody;
m_bodyId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
m_scale = 1.0f;
// b3Capsule Capsule = {
// { 1.88094640f, 1.13457823f, 30.2966614f }, { -1.37514091f, -2.26233172f, 848.671997f }, 51.7184296f
// };
b3Capsule capsule = {
.center1 = { 0.0f, 0.0f, 0.0f },
.center2 = { 0.0f, 1.0f, 0.0f },
.radius = 0.5f,
};
m_offset = capsule.center1;
b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule );
}
void Render() override
{
Sample::Render();
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
b3BodyRayCastInput input;
input.origin = { -1.0f, 0.5f, 0.0f };
input.translation = { 2.0f, 0.0f, 0.0f };
input.filter = b3DefaultQueryFilter();
input.maxFraction = 1.0f;
b3BodyCastResult result = b3Body_CastRay( m_bodyId, &input, b3Transform_identity );
DrawLine( input.origin, input.origin + input.translation, MakeColor( b3_colorGray ) );
DrawPoint( input.origin, 4.0f, MakeColor( b3_colorGreen ) );
DrawPoint( input.origin + input.translation, 4.0f, MakeColor( b3_colorRed ) );
if ( result.hit )
{
DrawPoint( result.point, 4.0f, MakeColor( b3_colorOrange ) );
}
}
b3BodyId m_bodyId;
b3Vec3 m_offset;
float m_scale;
};
static int sampleCapsuleCastRay = RegisterSample( "Collision", "Capsule Cast Ray", CapsuleCastRay::Create );