Added a compile time option to use double precision body positions. Most
math remains single precision with minimal performance loss.

`BOX3D_DOUBLE_PRECISION`

Improved accuracy of body inertia computation when shapes are far from
the body origin.

WARNING: several APIs changed but migration should be straight forward
if doubles are not enabled.
This commit is contained in:
Erin Catto 2026-06-18 20:51:16 -07:00 • committed by GitHub
commit 09c65b08ea
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
115 changed files with 7591 additions and 5295 deletions

View file

@ -1,10 +1,9 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "sample.h"
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "sample.h"
#include "box3d/box3d.h"
#include "box3d/constants.h"
@ -39,11 +38,11 @@ public:
m_simplexCache = {};
m_satCache = {};
m_manualFeature = 0;
m_baseTranslation = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_baseQuaternion = b3Quat_identity;
m_baseX = 0;
m_baseY = 0;
m_origin = b3Vec3_zero;
m_origin = b3Pos_zero;
m_useCache = false;
m_tracking = false;
m_rotating = false;
@ -54,7 +53,7 @@ public:
DrawTextLine( "origin: %g %g %g", m_origin.x, m_origin.y, m_origin.z );
DrawTextLine( "count = %d", m_manifold.pointCount );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
if ( m_manifold.pointCount == 0 )
{
@ -68,7 +67,7 @@ public:
{
const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex];
b3Vec3 point = b3TransformPoint( m_transformA, manifoldPoint.point );
b3Pos point = b3TransformWorldPoint( m_transformA, manifoldPoint.point );
DrawLine( point, point + length * normal, MakeColor( b3_colorWhite ) );
@ -81,11 +80,12 @@ public:
DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) );
}
DrawWorldString( point, MakeColor( b3_colorWhite ), " %.3f", manifoldPoint.separation );
DrawString3D( point, MakeColor( b3_colorWhite ), " %.3f", manifoldPoint.separation );
b3Vec3 perp = b3Perp( normal );
b3FeaturePair pair = manifoldPoint.pair;
DrawWorldString( point + 0.025f * normal + 0.05f * perp, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1, pair.owner2, pair.index2 );
DrawString3D( point + 0.025f * normal + 0.05f * perp, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1,
pair.index1, pair.owner2, pair.index2 );
}
Sample::Render();
@ -142,8 +142,8 @@ public:
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
if ( m_rotating )
@ -160,11 +160,11 @@ public:
static constexpr int m_pointCapacity = 64;
b3LocalManifold m_manifold;
b3LocalManifoldPoint m_points[m_pointCapacity];
b3Transform m_transformA;
b3Transform m_transformB;
b3Vec3 m_baseTranslation;
b3WorldTransform m_transformA;
b3WorldTransform m_transformB;
b3Pos m_baseTranslation;
b3Quat m_baseQuaternion;
b3Vec3 m_origin;
b3Pos m_origin;
b3SimplexCache m_simplexCache;
b3SATCache m_satCache;
int m_manualFeature;
@ -202,11 +202,11 @@ public:
m_simplexCache = {};
m_satCache = {};
m_baseTranslation = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_baseQuaternion = b3Quat_identity;
m_baseX = 0;
m_baseY = 0;
m_origin = b3Vec3_zero;
m_origin = b3Pos_zero;
m_useCache = false;
m_tracking = false;
m_rotating = false;
@ -221,19 +221,19 @@ public:
DrawTextLine( "feature = %d", m_manifold.feature );
DrawTextLine( "cache hit = %d", m_satCache.hit );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
if ( m_manifold.pointCount > 0 )
{
float length = 0.5f * b3GetLengthUnitsPerMeter();
b3Vec3 normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int pointIndex = 0; pointIndex < m_manifold.pointCount; ++pointIndex )
{
const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex];
b3Vec3 point = manifoldPoint.point;
DrawLine( point, point + length * m_manifold.normal, MakeColor( b3_colorWhite ) );
b3Pos point = b3TransformWorldPoint(m_transformB, manifoldPoint.point );
DrawLine( point, point + length * normal, MakeColor( b3_colorWhite ) );
if ( manifoldPoint.separation > 0.0f )
{
@ -244,24 +244,25 @@ public:
DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) );
}
DrawWorldString( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation );
DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation );
b3FeaturePair pair = manifoldPoint.pair;
DrawWorldString( point + 0.025f * m_manifold.normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1, pair.owner2, pair.index2 );
DrawString3D( point + 0.025f * normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1,
pair.index1, pair.owner2, pair.index2 );
}
}
b3Vec3 p1 = b3TransformPoint( m_transformA, m_triangle[0] );
b3Vec3 p2 = b3TransformPoint( m_transformA, m_triangle[1] );
b3Vec3 p3 = b3TransformPoint( m_transformA, m_triangle[2] );
DrawTriangle( p1, p2, p3, MakeColor( b3_colorCyan ) );
DrawTriangle( m_transformA, m_triangle[0], m_triangle[1], m_triangle[2], MakeColor( b3_colorCyan ) );
DrawWorldString( p1, MakeColor( b3_colorWhite ), "0" );
DrawWorldString( p2, MakeColor( b3_colorWhite ), "1" );
DrawWorldString( p3, MakeColor( b3_colorWhite ), "2" );
b3Pos p1 = b3TransformWorldPoint( m_transformA, m_triangle[0] );
b3Pos p2 = b3TransformWorldPoint( m_transformA, m_triangle[1] );
b3Pos p3 = b3TransformWorldPoint( m_transformA, m_triangle[2] );
DrawString3D( p1, MakeColor( b3_colorWhite ), "0" );
DrawString3D( p2, MakeColor( b3_colorWhite ), "1" );
DrawString3D( p3, MakeColor( b3_colorWhite ), "2" );
b3Vec3 center = 1.0f / 3.0f * ( p1 + p2 + p3 );
b3Vec3 normal = b3Normalize( b3Cross( p2 - p1, p3 - p1 ) );
b3Pos center = p1 + ( 1.0f / 3.0f ) * ( ( p2 - p1 ) + ( p3 - p1 ) );
DrawArrow( center, center + 0.5f * normal, MakeColor( b3_colorMediumPurple ) );
Sample::Render();
@ -318,8 +319,8 @@ public:
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
if ( m_rotating )
@ -336,12 +337,17 @@ public:
static constexpr int m_pointCapacity = 8;
b3LocalManifold m_manifold;
b3LocalManifoldPoint m_points[m_pointCapacity];
b3Transform m_transformA;
b3Transform m_transformB;
// Triangle transform
b3WorldTransform m_transformA;
// Convex shape transform
b3WorldTransform m_transformB;
b3Vec3 m_triangle[3] = {};
b3Vec3 m_baseTranslation;
b3Pos m_baseTranslation;
b3Quat m_baseQuaternion;
b3Vec3 m_origin;
b3Pos m_origin;
b3SimplexCache m_simplexCache;
b3SATCache m_satCache;
int m_manualFeature;
@ -371,7 +377,7 @@ public:
void Step() override
{
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideSpheres( &m_manifold, m_pointCapacity, &m_sphere, &m_sphere, transformBtoA );
}
@ -413,7 +419,7 @@ public:
void Step() override
{
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideCapsuleAndSphere( &m_manifold, m_pointCapacity, &m_capsule, &m_sphere, transformBtoA );
}
@ -451,7 +457,7 @@ public:
m_simplexCache = {};
}
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideHullAndSphere( &m_manifold, m_pointCapacity, &m_hull.base, &m_sphere, transformBtoA, &m_simplexCache );
}
@ -479,10 +485,10 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero );
}
m_sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f };
m_sphere = { { 0.0f, 0.0f, 0.0f }, 0.25f };
m_triangle[0] = { 0.0f, 0.0f, 0.0f };
m_triangle[1] = { 4.0f, 0.0f, 4.0f };
m_triangle[2] = { 4.0f, 0.0f, 0.0f };
@ -490,13 +496,13 @@ public:
// b3Quat qA = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 2.0f );
// m_transformA = { { 1.0f, 1.0f, 0.0f }, qA };
m_transformA = b3Transform_identity;
m_transformA = b3WorldTransform_identity;
m_transformB = { { 2.0f, 0.5f, 1.0f }, b3Quat_identity };
}
void Render() override
{
DrawSolidSphere( m_transformB, m_sphere, MakeColor( b3_colorGreen ) );
DrawSolidSphere( m_transformB, m_sphere, MakeColorAlpha( b3_colorGreen, 0.5f ) );
TriangleManifold::Render();
}
@ -504,7 +510,7 @@ public:
void Step() override
{
// Convert triangle to frame B
b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA );
b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA );
b3Vec3 localTriangle[3] = {
b3TransformPoint( xf, m_triangle[0] ),
b3TransformPoint( xf, m_triangle[1] ),
@ -512,13 +518,6 @@ public:
};
b3CollideSphereAndTriangle( &m_manifold, m_pointCapacity, &m_sphere, localTriangle );
// Convert manifold from frame B to world space
m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int i = 0; i < m_manifold.pointCount; ++i )
{
m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point );
}
}
b3Sphere m_sphere;
@ -548,7 +547,7 @@ public:
void Step() override
{
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideCapsules( &m_manifold, m_pointCapacity, &m_capsule, &m_capsule, transformBtoA );
}
@ -570,7 +569,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Pos_zero );
}
m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.5f };
@ -606,7 +605,7 @@ public:
m_simplexCache = {};
}
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideHullAndCapsule( &m_manifold, m_pointCapacity, &m_hull.base, &m_capsule, transformBtoA, &m_simplexCache );
}
@ -634,7 +633,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero );
}
m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f };
@ -642,7 +641,7 @@ public:
m_triangle[1] = { -4.0f, 0.0f, 0.0f };
m_triangle[2] = { 0.0f, 0.0f, 0.0f };
m_transformA = b3Transform_identity;
m_transformA = b3WorldTransform_identity;
m_transformB = { { 0.0f, 0.5f, 0.0f }, b3Quat_identity };
m_transformB.q = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.5f * B3_PI );
@ -652,7 +651,7 @@ public:
void Render() override
{
DrawSolidCapsule( m_transformB, m_capsule, MakeColor( b3_colorGreen ) );
DrawSolidCapsule( m_transformB, m_capsule, MakeColorAlpha( b3_colorGreen, 0.5f ) );
DrawAxes( m_transformB, 0.1f );
TriangleManifold::Render();
@ -666,7 +665,7 @@ public:
}
// Put triangle into capsule coordinates
b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA );
b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA );
b3Vec3 localTriangle[3] = {
b3TransformPoint( xf, m_triangle[0] ),
b3TransformPoint( xf, m_triangle[1] ),
@ -674,13 +673,6 @@ public:
};
b3CollideCapsuleAndTriangle( &m_manifold, m_pointCapacity, &m_capsule, localTriangle, &m_simplexCache );
// Convert manifold from frame B to world space
m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int i = 0; i < m_manifold.pointCount; ++i )
{
m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point );
}
}
b3Capsule m_capsule;
@ -696,7 +688,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Pos_zero );
}
// m_boxA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, { { 0.0f, -0.5f, 0.0f }, b3Quat_identity } );
@ -802,7 +794,7 @@ public:
m_satCache = {};
}
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, &m_satCache );
DrawTextLine( "SAT type: %d", m_satCache.type );
@ -834,7 +826,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 3.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 3.0f, b3Pos_zero );
}
m_triangle[0] = { 1.00000000, 0, 1.00000000 };
@ -843,8 +835,8 @@ public:
m_boxHull = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
m_transformA = b3Transform_identity;
m_transformB = b3Transform_identity;
m_transformA = b3WorldTransform_identity;
m_transformB = b3WorldTransform_identity;
m_transformB.p = { 0.0f, 0.45f, 0.1f };
// b3MeshEdgeFlags
@ -881,8 +873,8 @@ public:
{
DrawHull( m_transformB, m_hull, MakeColor( b3_colorGreen ) );
b3Transform xf;
xf.p = b3TransformPoint( m_transformB, m_hull->center );
b3WorldTransform xf;
xf.p = b3TransformWorldPoint( m_transformB, m_hull->center );
xf.q = m_transformB.q;
DrawAxes( xf, 0.1f );
@ -911,7 +903,7 @@ public:
m_satCache = {};
}
b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA );
b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA );
b3Vec3 localTriangle[3] = {
b3TransformPoint( xf, m_triangle[0] ),
b3TransformPoint( xf, m_triangle[1] ),
@ -920,13 +912,6 @@ public:
b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle[0], localTriangle[1], localTriangle[2],
m_flags, &m_satCache );
// Convert manifold from frame B to world space
m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int i = 0; i < m_manifold.pointCount; ++i )
{
m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point );
}
}
int m_flags;