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.
323 lines
9.5 KiB
C
323 lines
9.5 KiB
C
// SPDX-FileCopyrightText: 2023 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "test_macros.h"
|
|
|
|
#include "box3d/collision.h"
|
|
#include "box3d/math_functions.h"
|
|
|
|
#include <float.h>
|
|
|
|
static b3Capsule capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 1.0f };
|
|
static b3Sphere sphere = { { 1.0f, 0.0f, 0.0f }, 1.0f };
|
|
static b3BoxHull box = { 0 };
|
|
|
|
#define N 4
|
|
|
|
static int ShapeMassTest( void )
|
|
{
|
|
// Sphere
|
|
{
|
|
b3MassData md = b3ComputeSphereMass( &sphere, 1.0f );
|
|
float mass = 4.0f / 3.0f * B3_PI;
|
|
ENSURE_SMALL( md.mass - mass, FLT_EPSILON );
|
|
ENSURE( md.center.x == 1.0f && md.center.y == 0.0f );
|
|
|
|
// Inertia is now about the shape center of mass, so the offset does not appear.
|
|
float inertia = 2.0f / 5.0f * mass;
|
|
ENSURE_SMALL( md.inertia.cx.x - inertia, FLT_EPSILON );
|
|
ENSURE_SMALL( md.inertia.cy.y - inertia, FLT_EPSILON );
|
|
ENSURE_SMALL( md.inertia.cz.z - inertia, FLT_EPSILON );
|
|
}
|
|
|
|
// Analytic box hull
|
|
{
|
|
b3MassData md = b3ComputeHullMass( &box.base, 1.0f );
|
|
float mass = 2.0f * 2.0f * 2.0f;
|
|
ENSURE_SMALL( md.mass - mass, FLT_EPSILON );
|
|
ENSURE_SMALL( md.center.x, FLT_EPSILON );
|
|
ENSURE_SMALL( md.center.y, FLT_EPSILON );
|
|
ENSURE_SMALL( md.center.z, FLT_EPSILON );
|
|
float inertia = ( 1.0f / 12.0f ) * mass * ( 2.0f * 2.0f + 2.0f * 2.0f );
|
|
ENSURE_SMALL( md.inertia.cx.x - inertia, 2.0f * FLT_EPSILON );
|
|
ENSURE_SMALL( md.inertia.cy.y - inertia, 2.0f * FLT_EPSILON );
|
|
ENSURE_SMALL( md.inertia.cz.z - inertia, 2.0f * FLT_EPSILON );
|
|
}
|
|
|
|
// Translated box
|
|
{
|
|
b3Vec3 offset = { 0.4f, -0.7f, 0.1f };
|
|
b3Transform transform = {
|
|
.p = offset,
|
|
.q = b3Quat_identity,
|
|
};
|
|
b3Vec3 h = { 0.25f, 0.5f, 0.3f };
|
|
b3BoxHull b1 = b3MakeBoxHull( h.x, h.y, h.z );
|
|
b3BoxHull b2 = b3MakeTransformedBoxHull( h.x, h.y, h.z, transform );
|
|
|
|
b3MassData m1 = b3ComputeHullMass( &b1.base, 1.0f );
|
|
b3MassData m2 = b3ComputeHullMass( &b2.base, 1.0f );
|
|
|
|
ENSURE_SMALL( m1.mass - m2.mass, FLT_EPSILON );
|
|
|
|
b3Matrix3 d = b3SubMM( b1.base.centralInertia, b2.base.centralInertia );
|
|
ENSURE_SMALL( d.cx.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cx.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cx.z, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.z, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.z, FLT_EPSILON );
|
|
|
|
ENSURE_SMALL( m2.center.x - offset.x, FLT_EPSILON );
|
|
ENSURE_SMALL( m2.center.y - offset.y, FLT_EPSILON );
|
|
ENSURE_SMALL( m2.center.z - offset.z, FLT_EPSILON );
|
|
}
|
|
|
|
// Rotated box
|
|
{
|
|
b3Vec3 h1 = { 0.25f, 0.5f, 0.3f };
|
|
b3Vec3 h2 = { 0.25f, 0.3f, 0.5f };
|
|
b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ );
|
|
b3Transform transform = {
|
|
.p = b3Vec3_zero,
|
|
.q = q,
|
|
};
|
|
b3BoxHull b1 = b3MakeTransformedBoxHull( h1.x, h1.y, h1.z, transform );
|
|
b3BoxHull b2 = b3MakeBoxHull( h2.x, h2.y, h2.z );
|
|
|
|
b3MassData m1 = b3ComputeHullMass( &b1.base, 1.0f );
|
|
b3MassData m2 = b3ComputeHullMass( &b2.base, 1.0f );
|
|
|
|
ENSURE_SMALL( m1.mass - m2.mass, FLT_EPSILON );
|
|
|
|
b3Matrix3 d = b3SubMM( b1.base.centralInertia, b2.base.centralInertia );
|
|
ENSURE_SMALL( d.cx.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cx.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cx.z, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.z, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.z, FLT_EPSILON );
|
|
|
|
ENSURE_SMALL( m1.center.x - m2.center.x, FLT_EPSILON );
|
|
ENSURE_SMALL( m1.center.y - m2.center.y, FLT_EPSILON );
|
|
ENSURE_SMALL( m1.center.z - m2.center.z, FLT_EPSILON );
|
|
}
|
|
|
|
// Transformed box
|
|
{
|
|
b3Vec3 offset = { 0.4f, -0.7f, 0.1f };
|
|
b3Vec3 h1 = { 0.25f, 0.5f, 0.3f };
|
|
b3Vec3 h2 = { 0.25f, 0.3f, 0.5f };
|
|
b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ );
|
|
b3Transform transform = {
|
|
.p = offset,
|
|
.q = q,
|
|
};
|
|
b3BoxHull b1 = b3MakeTransformedBoxHull( h1.x, h1.y, h1.z, transform );
|
|
b3BoxHull b2 = b3MakeBoxHull( h2.x, h2.y, h2.z );
|
|
|
|
b3MassData m1 = b3ComputeHullMass( &b1.base, 1.0f );
|
|
b3MassData m2 = b3ComputeHullMass( &b2.base, 1.0f );
|
|
|
|
ENSURE_SMALL( m1.mass - m2.mass, FLT_EPSILON );
|
|
|
|
b3Matrix3 d = b3SubMM( b1.base.centralInertia, b2.base.centralInertia );
|
|
ENSURE_SMALL( d.cx.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cx.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cx.z, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cy.z, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.x, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.y, FLT_EPSILON );
|
|
ENSURE_SMALL( d.cz.z, FLT_EPSILON );
|
|
|
|
ENSURE_SMALL( m1.center.x - offset.x, FLT_EPSILON );
|
|
ENSURE_SMALL( m1.center.y - offset.y, FLT_EPSILON );
|
|
ENSURE_SMALL( m1.center.z - offset.z, FLT_EPSILON );
|
|
}
|
|
|
|
// Capsule
|
|
{
|
|
float radius = capsule.radius;
|
|
float length = b3Distance( capsule.center1, capsule.center2 );
|
|
|
|
// Capsule along x-axis
|
|
b3MassData md = b3ComputeCapsuleMass( &capsule, 1.0f );
|
|
|
|
// Box that fully contains capsule. Upper bound on capsule mass.
|
|
b3BoxHull r = b3MakeBoxHull( radius + 0.5f * length, radius, radius );
|
|
b3MassData mdUpper = b3ComputeHullMass( &r.base, 1.0f );
|
|
|
|
// Approximate capsule using convex hull. This should be a lower bound on the
|
|
// capsule mass.
|
|
b3Vec3 points[2 * N * N];
|
|
float d = B3_PI / ( N - 1.0f );
|
|
float angle1 = -0.5f * B3_PI;
|
|
int index = 0;
|
|
for ( int i = 0; i < N; ++i )
|
|
{
|
|
float s1 = sinf( angle1 );
|
|
float c1 = cosf( angle1 );
|
|
float angle2 = -0.5f * B3_PI;
|
|
for ( int j = 0; j < N; ++j )
|
|
{
|
|
points[index].x = 1.0f + radius * c1;
|
|
points[index].y = radius * s1 * cosf( angle2 );
|
|
points[index].z = radius * s1 * sinf( angle2 );
|
|
angle2 += d;
|
|
index += 1;
|
|
}
|
|
|
|
angle1 += d;
|
|
}
|
|
|
|
angle1 = 0.5f * B3_PI;
|
|
for ( int i = 0; i < N; ++i )
|
|
{
|
|
float s1 = sinf( angle1 );
|
|
float c1 = cosf( angle1 );
|
|
float angle2 = -0.5f * B3_PI;
|
|
for ( int j = 0; j < N; ++j )
|
|
{
|
|
points[index].x = -1.0f + radius * c1;
|
|
points[index].y = radius * s1 * cosf( angle2 );
|
|
points[index].z = radius * s1 * sinf( angle2 );
|
|
angle2 += d;
|
|
index += 1;
|
|
}
|
|
|
|
angle1 += d;
|
|
}
|
|
|
|
ENSURE( index == 2 * N * N );
|
|
|
|
b3HullData* hull = b3CreateHull( points, 2 * N * N, 2 * N * N );
|
|
b3MassData mdLower = b3ComputeHullMass( hull, 1.0f );
|
|
|
|
ENSURE( mdLower.mass < md.mass && md.mass < mdUpper.mass );
|
|
ENSURE( mdLower.inertia.cx.x < md.inertia.cx.x && md.inertia.cx.x < mdUpper.inertia.cx.x );
|
|
ENSURE( mdLower.inertia.cy.y < md.inertia.cy.y && md.inertia.cy.y < mdUpper.inertia.cy.y );
|
|
ENSURE( mdLower.inertia.cz.z < md.inertia.cz.z && md.inertia.cz.z < mdUpper.inertia.cz.z );
|
|
|
|
b3DestroyHull( hull );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int ShapeAABBTest( void )
|
|
{
|
|
{
|
|
b3AABB b = b3ComputeSphereAABB( &sphere, b3Transform_identity );
|
|
ENSURE_SMALL( b.lowerBound.x, FLT_EPSILON );
|
|
ENSURE_SMALL( b.lowerBound.y + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.lowerBound.z + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.x - 2.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.y - 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.z - 1.0f, FLT_EPSILON );
|
|
}
|
|
|
|
{
|
|
b3AABB b = b3ComputeCapsuleAABB( &capsule, b3Transform_identity );
|
|
ENSURE_SMALL( b.lowerBound.x + 2.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.lowerBound.y + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.lowerBound.z + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.x - 2.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.y - 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.z - 1.0f, FLT_EPSILON );
|
|
}
|
|
{
|
|
b3AABB b = b3ComputeHullAABB( &box.base, b3Transform_identity );
|
|
ENSURE_SMALL( b.lowerBound.x + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.lowerBound.y + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.lowerBound.z + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.x - 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.y - 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( b.upperBound.z - 1.0f, FLT_EPSILON );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
#if 0
|
|
static int PointInShapeTest( void )
|
|
{
|
|
b3Vec2 p1 = { 0.5f, 0.5f };
|
|
b3Vec2 p2 = { 4.0f, -4.0f };
|
|
|
|
{
|
|
bool hit;
|
|
hit = b3PointInSphere( p1, &sphere );
|
|
ENSURE( hit == true );
|
|
hit = b3PointInSphere( p2, &sphere );
|
|
ENSURE( hit == false );
|
|
}
|
|
|
|
{
|
|
bool hit;
|
|
hit = b3PointInPolygon( p1, &box );
|
|
ENSURE( hit == true );
|
|
hit = b3PointInPolygon( p2, &box );
|
|
ENSURE( hit == false );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
static int RayCastShapeTest( void )
|
|
{
|
|
b3RayCastInput input = {
|
|
.origin = { -4.0f, 0.0f, 0.0f },
|
|
.translation = { 8.0f, 0.0f, 0.0f },
|
|
.maxFraction = 1.0f,
|
|
};
|
|
|
|
{
|
|
b3CastOutput output = b3RayCastSphere( &sphere, &input );
|
|
ENSURE( output.hit );
|
|
ENSURE_SMALL( output.normal.x + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( output.normal.y, FLT_EPSILON );
|
|
ENSURE_SMALL( output.normal.z, FLT_EPSILON );
|
|
ENSURE_SMALL( output.fraction - 0.5f, FLT_EPSILON );
|
|
}
|
|
|
|
{
|
|
b3CastOutput output = b3RayCastCapsule( &capsule, &input );
|
|
ENSURE( output.hit );
|
|
ENSURE_SMALL( output.normal.x + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( output.normal.y, FLT_EPSILON );
|
|
ENSURE_SMALL( output.normal.z, FLT_EPSILON );
|
|
ENSURE_SMALL( output.fraction - 1.0f / 4.0f, FLT_EPSILON );
|
|
}
|
|
|
|
{
|
|
b3CastOutput output = b3RayCastHull( &box.base, &input );
|
|
ENSURE( output.hit );
|
|
ENSURE_SMALL( output.normal.x + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( output.normal.y, FLT_EPSILON );
|
|
ENSURE_SMALL( output.normal.z, FLT_EPSILON );
|
|
ENSURE_SMALL( output.fraction - 3.0f / 8.0f, FLT_EPSILON );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int ShapeTest( void )
|
|
{
|
|
box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f );
|
|
|
|
RUN_SUBTEST( ShapeMassTest );
|
|
RUN_SUBTEST( ShapeAABBTest );
|
|
// RUN_SUBTEST( PointInShapeTest );
|
|
RUN_SUBTEST( RayCastShapeTest );
|
|
|
|
return 0;
|
|
}
|