// SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT #include "test_macros.h" #include "box3d/collision.h" #include "box3d/math_functions.h" #include 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 ); float inertia = 2.0f / 5.0f * mass; float offsetInertia = inertia + mass; ENSURE_SMALL( md.inertia.cx.x - inertia, FLT_EPSILON ); ENSURE_SMALL( md.inertia.cy.y - offsetInertia, FLT_EPSILON ); ENSURE_SMALL( md.inertia.cz.z - offsetInertia, 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 ); b3Hull* 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; }