Doubles (#14)
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:
parent
a252f03b19
commit
09c65b08ea
115 changed files with 7591 additions and 5295 deletions
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in a new issue