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,13 +1,18 @@
|
|||
// SPDX-FileCopyrightText: 2025 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "earcut.h"
|
||||
#include "gfx/draw.h"
|
||||
#include "gfx/keycodes.h"
|
||||
#include "imgui.h"
|
||||
#include "sample.h"
|
||||
#include "utils.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
// Test the distance joint and all options
|
||||
class DistanceJoint : public Sample
|
||||
{
|
||||
|
|
@ -92,8 +97,8 @@ public:
|
|||
m_bodyIds[i] = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateSphereShape( m_bodyIds[i], &shapeDef, &sphere );
|
||||
|
||||
b3Vec3 pivotA = { m_length * i, yOffset, 0.0f };
|
||||
b3Vec3 pivotB = { m_length * ( i + 1.0f ), yOffset, 0.0f };
|
||||
b3Pos pivotA = { m_length * i, yOffset, 0.0f };
|
||||
b3Pos pivotB = { m_length * ( i + 1.0f ), yOffset, 0.0f };
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[i];
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevBodyId, pivotA );
|
||||
|
|
@ -334,7 +339,7 @@ public:
|
|||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { -2.0f, 2.0f, 0.0f };
|
||||
bodyDef.position = { -2.0, 2.0, 0.0 };
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
|
|
@ -344,7 +349,7 @@ public:
|
|||
b3MotorJointDef jointDef = b3DefaultMotorJointDef();
|
||||
jointDef.base.bodyIdA = groundId;
|
||||
jointDef.base.bodyIdB = bodyId;
|
||||
jointDef.base.localFrameA.p = b3Add( bodyDef.position, { 0.25f, 1.25f, 0.0f } );
|
||||
jointDef.base.localFrameA.p = { -1.75f, 3.25f, 0.0f };
|
||||
jointDef.base.localFrameB.p = { 0.25f, 0.25f };
|
||||
jointDef.base.collideConnected = true;
|
||||
jointDef.linearHertz = 7.5f;
|
||||
|
|
@ -401,7 +406,7 @@ public:
|
|||
{
|
||||
m_time += m_speed * timeStep;
|
||||
|
||||
b3Vec3 linearOffset;
|
||||
b3Pos linearOffset;
|
||||
linearOffset.x = 6.0f * sinf( 2.0f * m_time );
|
||||
linearOffset.y = 10.0f + 4.0f * sinf( 1.0f * m_time );
|
||||
linearOffset.z = 0.0f;
|
||||
|
|
@ -430,7 +435,7 @@ public:
|
|||
b3BodyId m_targetId;
|
||||
b3BodyId m_bodyId;
|
||||
b3JointId m_jointId;
|
||||
b3Transform m_transform;
|
||||
b3WorldTransform m_transform;
|
||||
float m_time;
|
||||
float m_speed;
|
||||
float m_maxForce;
|
||||
|
|
@ -524,13 +529,13 @@ public:
|
|||
{
|
||||
b3Sphere sphere = { { 0.0f, 10.0f, 0.0 }, 10.0f };
|
||||
b3ExplosionDef def = b3DefaultExplosionDef();
|
||||
def.position = sphere.center;
|
||||
def.position = { 0.0, 10.0, 0.0 };
|
||||
def.radius = sphere.radius;
|
||||
def.falloff = 5.0f;
|
||||
def.impulsePerArea = 10000.0f;
|
||||
b3World_Explode( m_worldId, &def );
|
||||
|
||||
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( b3_colorWhite ) );
|
||||
DrawSolidSphere( b3WorldTransform_identity, sphere, MakeColor( b3_colorWhite ) );
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -1156,7 +1161,7 @@ public:
|
|||
b3Vec3 linearVelocity = b3Body_GetLinearVelocity( m_bodyId );
|
||||
float kineticEnergy = 0.5f * b3Dot( angularVelocity, b3MulMV( massData.inertia, angularVelocity ) );
|
||||
kineticEnergy += 0.5f * massData.mass * b3Dot( linearVelocity, linearVelocity );
|
||||
b3Vec3 center = b3Body_GetWorldCenterOfMass( m_bodyId );
|
||||
b3Pos center = b3Body_GetWorldCenterOfMass( m_bodyId );
|
||||
b3Vec3 gravity = b3World_GetGravity( m_worldId );
|
||||
float potentialEnergy = -massData.mass * center.y * gravity.y;
|
||||
DrawTextLine( "kinetic energy = %g", kineticEnergy );
|
||||
|
|
@ -1738,7 +1743,7 @@ public:
|
|||
|
||||
if ( ImGui::Button( "Impulse##Door" ) )
|
||||
{
|
||||
b3Vec3 p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
|
||||
b3Pos p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
|
||||
b3Body_ApplyLinearImpulse( m_doorId, { 0.0f, 0.0f, -m_magnitude }, p, true );
|
||||
m_translationError1 = 0.0f;
|
||||
m_translationError2 = 0.0f;
|
||||
|
|
@ -1790,7 +1795,7 @@ public:
|
|||
{
|
||||
Sample::Step();
|
||||
|
||||
b3Vec3 p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
|
||||
b3Pos p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
|
||||
DrawPoint( p, 10.0f, MakeColor( b3_colorDarkKhaki ) );
|
||||
|
||||
float translationError1 = b3Joint_GetLinearSeparation( m_jointId1 );
|
||||
|
|
@ -1876,7 +1881,7 @@ public:
|
|||
b3CreateHullShape( m_bodyIds[i], &shapeDef, &box.base );
|
||||
|
||||
{
|
||||
b3Vec3 pivot = { xbase + 2.0f * a * i, 20.0f, -0.5f };
|
||||
b3Pos pivot = { xbase + 2.0f * a * i, 20.0, -0.5 };
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[i];
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
|
||||
|
|
@ -1886,7 +1891,7 @@ public:
|
|||
}
|
||||
|
||||
{
|
||||
b3Vec3 pivot = { xbase + 2.0f * a * i, 20.0f, 0.5f };
|
||||
b3Pos pivot = { xbase + 2.0f * a * i, 20.0, 0.5 };
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[i];
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
|
||||
|
|
@ -1899,7 +1904,7 @@ public:
|
|||
}
|
||||
|
||||
{
|
||||
b3Vec3 pivot = { xbase + 2.0f * a * m_count, 20.0f, -0.5f };
|
||||
b3Pos pivot = { xbase + 2.0f * a * m_count, 20.0, -0.5 };
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = groundId;
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
|
||||
|
|
@ -1909,7 +1914,7 @@ public:
|
|||
}
|
||||
|
||||
{
|
||||
b3Vec3 pivot = { xbase + 2.0f * a * m_count, 20.0f, 0.5f };
|
||||
b3Pos pivot = { xbase + 2.0f * a * m_count, 20.0, 0.5 };
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = groundId;
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
|
||||
|
|
@ -1975,7 +1980,7 @@ public:
|
|||
m_bodyIds[i] = b3_nullBodyId;
|
||||
}
|
||||
|
||||
b3Vec3 position = { -12.5f, 10.0, 0.0f };
|
||||
b3Pos position = { -12.5f, 10.0, 0.0f };
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.enableSleep = false;
|
||||
|
|
@ -1999,8 +2004,8 @@ public:
|
|||
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
|
||||
|
||||
float length = 2.0f;
|
||||
b3Vec3 pivot1 = { position.x, position.y + 1.0f + length, 0.0f };
|
||||
b3Vec3 pivot2 = { position.x, position.y + 1.0f, 0.0f };
|
||||
b3Pos pivot1 = { position.x, position.y + 1.0f + length, 0.0f };
|
||||
b3Pos pivot2 = { position.x, position.y + 1.0f, 0.0f };
|
||||
b3DistanceJointDef jointDef = b3DefaultDistanceJointDef();
|
||||
jointDef.base.bodyIdA = groundId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[m_count];
|
||||
|
|
@ -2051,7 +2056,7 @@ public:
|
|||
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
|
||||
|
||||
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
|
||||
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
|
||||
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
|
||||
jointDef.base.bodyIdA = groundId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[m_count];
|
||||
|
|
@ -2075,7 +2080,7 @@ public:
|
|||
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
|
||||
|
||||
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
|
||||
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
|
||||
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
|
||||
jointDef.base.bodyIdA = groundId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[m_count];
|
||||
|
|
@ -2099,7 +2104,7 @@ public:
|
|||
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
|
||||
|
||||
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
|
||||
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
|
||||
b3WeldJointDef jointDef = b3DefaultWeldJointDef();
|
||||
jointDef.base.bodyIdA = groundId;
|
||||
jointDef.base.bodyIdB = m_bodyIds[m_count];
|
||||
|
|
@ -2557,7 +2562,7 @@ public:
|
|||
float rightSteerTorque = b3WheelJoint_GetSteeringTorque( m_frontRightId );
|
||||
DrawTextLine( "steering torque = %.1f/%.1f", leftSteerTorque, rightSteerTorque );
|
||||
|
||||
b3Transform transform = b3Transform_identity;
|
||||
b3WorldTransform transform = b3WorldTransform_identity;
|
||||
transform.p.y += 0.05f;
|
||||
DrawAxes( transform, 2.0f );
|
||||
}
|
||||
|
|
@ -2602,7 +2607,7 @@ public:
|
|||
|
||||
if ( m_camera->m_thirdPerson )
|
||||
{
|
||||
b3Transform transform = b3Body_GetTransform( m_chassisId );
|
||||
b3WorldTransform transform = b3Body_GetTransform( m_chassisId );
|
||||
m_camera->m_pivot = transform.p;
|
||||
m_camera->UpdateTransform();
|
||||
}
|
||||
|
|
@ -2641,3 +2646,479 @@ public:
|
|||
};
|
||||
|
||||
static int sampleDriving = RegisterSample( "Joints", "Driving", Driving::Create );
|
||||
|
||||
// https://www.linkedin.com/feed/update/urn:li:activity:7308603994082353153/
|
||||
class GearLift : public Sample
|
||||
{
|
||||
public:
|
||||
explicit GearLift( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 18.0f, 12.0f, 17.0f, { -1.5f, 4.5f, 0.0f } );
|
||||
}
|
||||
|
||||
AddGroundBox( 20.0f );
|
||||
|
||||
b3BodyId groundId;
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
groundId = b3CreateBody( m_worldId, &bodyDef );
|
||||
}
|
||||
|
||||
CreateMesh( groundId );
|
||||
|
||||
// Gear cores shared by both shafts: a disk at each depth with an axle bridging
|
||||
// them, all cylinders along z so the gears spin about z.
|
||||
b3HullData* diskNear = MakeZCylinder( m_gearRadius, -m_gearZ - m_gearHalfDepth, -m_gearZ + m_gearHalfDepth, m_gearSides );
|
||||
b3HullData* diskFar = MakeZCylinder( m_gearRadius, m_gearZ - m_gearHalfDepth, m_gearZ + m_gearHalfDepth, m_gearSides );
|
||||
b3HullData* axle = MakeZCylinder( m_axleRadius, -m_gearZ, m_gearZ, m_axleSides );
|
||||
|
||||
m_motorTorque = 30000.0f;
|
||||
m_motorSpeed = -0.3f;
|
||||
m_enableMotor = true;
|
||||
|
||||
b3Pos gearPosition1 = { -4.25f, 9.75f, 0.0f };
|
||||
b3Pos gearPosition2 = { -2.25f, 10.75f, 0.0f };
|
||||
|
||||
b3BodyId driverId = BuildGearBody( gearPosition1, m_gearRadius + m_toothHalfHeight, diskNear, diskFar, axle );
|
||||
b3BodyId followerId = BuildGearBody( gearPosition2, m_gearRadius + m_toothHalfWidth, diskNear, diskFar, axle );
|
||||
|
||||
b3DestroyHull( diskNear );
|
||||
b3DestroyHull( diskFar );
|
||||
b3DestroyHull( axle );
|
||||
|
||||
// Driver shaft, motorized
|
||||
{
|
||||
b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef();
|
||||
revoluteDef.base.bodyIdA = groundId;
|
||||
revoluteDef.base.bodyIdB = driverId;
|
||||
revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, gearPosition1 );
|
||||
revoluteDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f };
|
||||
revoluteDef.enableMotor = m_enableMotor;
|
||||
revoluteDef.maxMotorTorque = m_motorTorque;
|
||||
revoluteDef.motorSpeed = m_motorSpeed;
|
||||
m_driverId = b3CreateRevoluteJoint( m_worldId, &revoluteDef );
|
||||
}
|
||||
|
||||
// Follower shaft, swept between angle limits
|
||||
{
|
||||
b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef();
|
||||
revoluteDef.base.bodyIdA = groundId;
|
||||
revoluteDef.base.bodyIdB = followerId;
|
||||
revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, gearPosition2 );
|
||||
revoluteDef.base.localFrameA.q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
|
||||
revoluteDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f };
|
||||
revoluteDef.enableMotor = true;
|
||||
revoluteDef.maxMotorTorque = 0.5f;
|
||||
revoluteDef.lowerAngle = -0.3f * B3_PI;
|
||||
revoluteDef.upperAngle = 0.8f * B3_PI;
|
||||
revoluteDef.enableLimit = true;
|
||||
b3CreateRevoluteJoint( m_worldId, &revoluteDef );
|
||||
}
|
||||
|
||||
// One chain hangs from the follower rim at each depth and both lift the gate.
|
||||
b3Pos linkAttach = { gearPosition2.x + m_gearRadius + 2.0f * m_toothHalfWidth + m_toothRadius, gearPosition2.y, 0.0f };
|
||||
b3Pos doorPosition = { linkAttach.x, linkAttach.y - ( 2.0f * m_linkCount * m_linkHalfLength + m_doorHalfHeight ), 0.0f };
|
||||
|
||||
b3BodyId nearLink = CreateChain( followerId, { linkAttach.x, linkAttach.y, -m_gearZ } );
|
||||
b3BodyId farLink = CreateChain( followerId, { linkAttach.x, linkAttach.y, m_gearZ } );
|
||||
|
||||
CreateDoor( groundId, doorPosition, nearLink, farLink );
|
||||
|
||||
CreateDebris();
|
||||
}
|
||||
|
||||
~GearLift() override
|
||||
{
|
||||
b3DestroyMesh( m_mesh );
|
||||
}
|
||||
|
||||
void CreateMesh( b3BodyId groundId )
|
||||
{
|
||||
// Silhouette of the Box2D stairwell traced as a closed loop. It is extruded
|
||||
// four meters along z into a single triangle mesh, the 3D analogue of a
|
||||
// b2Chain loop. The solid is inside the loop, so the windings face outward
|
||||
// to put the collision normals on the open basin side where the debris sits.
|
||||
static const b3Vec2 points[32] = {
|
||||
{ -11.3000f, -0.2167f }, { 9.3375f, -0.2167f }, { 9.3375f, 7.1917f }, { 8.8083f, 7.1917f }, { 8.8083f, 0.3125f },
|
||||
{ 0.3417f, 0.3125f }, { 0.3417f, 0.8417f }, { -0.1875f, 0.8417f }, { -0.1875f, 1.3708f }, { -0.7167f, 1.3708f },
|
||||
{ -0.7167f, 1.9000f }, { -1.2458f, 1.9000f }, { -1.2458f, 2.4292f }, { -1.7750f, 2.4292f }, { -1.7750f, 2.9583f },
|
||||
{ -2.3042f, 2.9583f }, { -2.3042f, 3.4875f }, { -2.8333f, 3.4875f }, { -2.8333f, 4.0167f }, { -3.3625f, 4.0167f },
|
||||
{ -3.3625f, 4.5458f }, { -3.8917f, 4.5458f }, { -3.8917f, 5.0750f }, { -4.4208f, 5.0750f }, { -4.4208f, 5.6042f },
|
||||
{ -4.9500f, 5.6042f }, { -4.9500f, 6.1333f }, { -5.4792f, 6.1333f }, { -5.4792f, 6.6625f }, { -6.0083f, 6.6625f },
|
||||
{ -6.0083f, 7.1917f }, { -11.3000f, 7.1917f },
|
||||
};
|
||||
|
||||
float zMin = -2.0f; // four meters across z
|
||||
float zMax = 2.0f;
|
||||
|
||||
// Two vertices per silhouette point, one at each depth.
|
||||
b3Vec3 vertices[64];
|
||||
for ( int i = 0; i < 32; ++i )
|
||||
{
|
||||
vertices[2 * i + 0] = { points[i].x, points[i].y, zMin };
|
||||
vertices[2 * i + 1] = { points[i].x, points[i].y, zMax };
|
||||
}
|
||||
|
||||
std::vector<int> indices;
|
||||
|
||||
// Side walls: two triangles per silhouette edge, wound so the normal faces
|
||||
// the open basin. The lo ring sits at zMin, the hi ring at zMax.
|
||||
for ( int i = 0; i < 32; ++i )
|
||||
{
|
||||
int j = ( i + 1 ) % 32;
|
||||
int aLo = 2 * i, aHi = 2 * i + 1;
|
||||
int bLo = 2 * j, bHi = 2 * j + 1;
|
||||
|
||||
indices.push_back( aLo );
|
||||
indices.push_back( bLo );
|
||||
indices.push_back( bHi );
|
||||
|
||||
indices.push_back( aLo );
|
||||
indices.push_back( bHi );
|
||||
indices.push_back( aHi );
|
||||
}
|
||||
|
||||
// End caps close the prism into a watertight manifold with no boundary edges.
|
||||
// The non convex silhouette is triangulated with earcut, then each triangle is
|
||||
// wound so the cap normal points out of the solid: the zMin cap faces -z and
|
||||
// the zMax cap faces +z. The two ring edges each gain a second owner this way.
|
||||
std::vector<std::array<double, 2>> ring;
|
||||
ring.reserve( 32 );
|
||||
for ( int i = 0; i < 32; ++i )
|
||||
{
|
||||
ring.push_back( { points[i].x, points[i].y } );
|
||||
}
|
||||
std::vector<std::vector<std::array<double, 2>>> polygon = { ring };
|
||||
std::vector<uint32_t> cap = mapbox::earcut<uint32_t>( polygon );
|
||||
|
||||
for ( size_t k = 0; k + 3 <= cap.size(); k += 3 )
|
||||
{
|
||||
int r0 = (int)cap[k], r1 = (int)cap[k + 1], r2 = (int)cap[k + 2];
|
||||
PushCap( indices, points, r0, r1, r2, 1, true ); // zMax cap, +z out
|
||||
PushCap( indices, points, r0, r1, r2, 0, false ); // zMin cap, -z out
|
||||
}
|
||||
|
||||
b3MeshDef def = {};
|
||||
def.vertices = vertices;
|
||||
def.vertexCount = 64;
|
||||
def.indices = indices.data();
|
||||
def.triangleCount = (int)( indices.size() / 3 );
|
||||
def.identifyEdges = true;
|
||||
|
||||
m_mesh = b3CreateMesh( &def, nullptr, 0 );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.customColor = b3_colorDarkSeaGreen;
|
||||
b3SurfaceMaterial material = shapeDef.baseMaterial;
|
||||
shapeDef.materials = &material;
|
||||
shapeDef.materialCount = 1;
|
||||
b3CreateMeshShape( groundId, &shapeDef, m_mesh, b3Vec3_one );
|
||||
|
||||
// Back wall: a 0.1 m thick box closing the far side over the full mesh extent.
|
||||
b3Vec2 lower = points[0];
|
||||
b3Vec2 upper = points[0];
|
||||
for ( int i = 1; i < 32; ++i )
|
||||
{
|
||||
lower.x = b3MinFloat( lower.x, points[i].x );
|
||||
lower.y = b3MinFloat( lower.y, points[i].y );
|
||||
upper.x = b3MaxFloat( upper.x, points[i].x );
|
||||
upper.y = b3MaxFloat( upper.y, points[i].y );
|
||||
}
|
||||
|
||||
float wallHalfThick = 0.05f;
|
||||
b3Vec3 wallCenter = { 0.5f * ( lower.x + upper.x ), 0.5f * ( lower.y + upper.y ), -zMax - wallHalfThick };
|
||||
b3BoxHull wall =
|
||||
b3MakeOffsetBoxHull( 0.5f * ( upper.x - lower.x ), 0.5f * ( upper.y - lower.y ), wallHalfThick, wallCenter );
|
||||
b3CreateHullShape( groundId, &shapeDef, &wall.base );
|
||||
}
|
||||
|
||||
// Push one cap triangle, flipping the winding so its z-normal has the wanted sign.
|
||||
void PushCap( std::vector<int>& indices, const b3Vec2* poly, int r0, int r1, int r2, int vOffset, bool wantPositiveZ )
|
||||
{
|
||||
float cross =
|
||||
( poly[r1].x - poly[r0].x ) * ( poly[r2].y - poly[r0].y ) - ( poly[r1].y - poly[r0].y ) * ( poly[r2].x - poly[r0].x );
|
||||
bool positive = cross > 0.0f;
|
||||
|
||||
int v0 = 2 * r0 + vOffset;
|
||||
int v1 = 2 * r1 + vOffset;
|
||||
int v2 = 2 * r2 + vOffset;
|
||||
|
||||
indices.push_back( v0 );
|
||||
if ( positive == wantPositiveZ )
|
||||
{
|
||||
indices.push_back( v1 );
|
||||
indices.push_back( v2 );
|
||||
}
|
||||
else
|
||||
{
|
||||
indices.push_back( v2 );
|
||||
indices.push_back( v1 );
|
||||
}
|
||||
}
|
||||
|
||||
// A cylinder built directly along z (depth) so it needs no reorientation.
|
||||
b3HullData* MakeZCylinder( float radius, float zMin, float zMax, int sides )
|
||||
{
|
||||
b3Vec3 points[64];
|
||||
for ( int i = 0; i < sides; ++i )
|
||||
{
|
||||
float angle = ( 2.0f * B3_PI * i ) / sides;
|
||||
float c = cosf( angle );
|
||||
float s = sinf( angle );
|
||||
points[2 * i + 0] = { radius * c, radius * s, zMin };
|
||||
points[2 * i + 1] = { radius * c, radius * s, zMax };
|
||||
}
|
||||
|
||||
return b3CreateHull( points, 2 * sides, 2 * sides );
|
||||
}
|
||||
|
||||
// One gear shaft: a disk and tooth ring at each depth joined by an axle, all on
|
||||
// one rigid body so the two depth gears share an angle and read as a clean gear.
|
||||
b3BodyId BuildGearBody( b3Pos position, float toothCenterRadius, b3HullData* diskNear, b3HullData* diskFar, b3HullData* axle )
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = position;
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.1f;
|
||||
shapeDef.baseMaterial.customColor = b3_colorSaddleBrown;
|
||||
b3CreateHullShape( bodyId, &shapeDef, diskNear );
|
||||
b3CreateHullShape( bodyId, &shapeDef, diskFar );
|
||||
|
||||
shapeDef.baseMaterial.customColor = b3_colorSlateGray;
|
||||
b3CreateHullShape( bodyId, &shapeDef, axle );
|
||||
|
||||
AddTeeth( bodyId, toothCenterRadius, -m_gearZ );
|
||||
AddTeeth( bodyId, toothCenterRadius, m_gearZ );
|
||||
|
||||
return bodyId;
|
||||
}
|
||||
|
||||
// Chain of capsule links hanging from a body down to the gate. Returns the last link.
|
||||
b3BodyId CreateChain( b3BodyId topBodyId, b3Pos attach )
|
||||
{
|
||||
b3Capsule capsule = { { 0.0f, -m_linkHalfLength, 0.0f }, { 0.0f, m_linkHalfLength, 0.0f }, m_linkRadius };
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.customColor = b3_colorLightSteelBlue;
|
||||
|
||||
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
|
||||
jointDef.maxMotorTorque = 0.05f;
|
||||
jointDef.enableMotor = true;
|
||||
jointDef.base.drawScale = 0.2f;
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
b3Pos position = { attach.x, attach.y - m_linkHalfLength, attach.z };
|
||||
|
||||
b3BodyId prevBodyId = topBodyId;
|
||||
for ( int i = 0; i < m_linkCount; ++i )
|
||||
{
|
||||
bodyDef.position = position;
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3CreateCapsuleShape( bodyId, &shapeDef, &capsule );
|
||||
|
||||
b3Pos pivot = { position.x, position.y + m_linkHalfLength, attach.z };
|
||||
jointDef.base.bodyIdA = prevBodyId;
|
||||
jointDef.base.bodyIdB = bodyId;
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevBodyId, pivot );
|
||||
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( bodyId, pivot );
|
||||
b3CreateRevoluteJoint( m_worldId, &jointDef );
|
||||
|
||||
position.y -= 2.0f * m_linkHalfLength;
|
||||
prevBodyId = bodyId;
|
||||
}
|
||||
|
||||
return prevBodyId;
|
||||
}
|
||||
|
||||
// A single gate box raised by both chains and held upright sliding along y.
|
||||
void CreateDoor( b3BodyId groundId, b3Pos doorPosition, b3BodyId nearLink, b3BodyId farLink )
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = doorPosition;
|
||||
b3BodyId doorId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.density *= 0.5f;
|
||||
shapeDef.baseMaterial.friction = 0.1f;
|
||||
shapeDef.baseMaterial.customColor = b3MakeDebugColor( b3_colorDarkCyan, b3_debugMaterialMetallic );
|
||||
b3BoxHull box = b3MakeBoxHull( 0.05f, m_doorHalfHeight, m_doorHalfDepth );
|
||||
b3CreateHullShape( doorId, &shapeDef, &box.base );
|
||||
|
||||
// Hinge the gate to each chain at its depth.
|
||||
b3BodyId links[2] = { nearLink, farLink };
|
||||
float depths[2] = { -m_gearZ, m_gearZ };
|
||||
for ( int i = 0; i < 2; ++i )
|
||||
{
|
||||
b3Pos pivot = { doorPosition.x, doorPosition.y + m_doorHalfHeight, depths[i] };
|
||||
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
|
||||
jointDef.base.bodyIdA = links[i];
|
||||
jointDef.base.bodyIdB = doorId;
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( links[i], pivot );
|
||||
jointDef.base.localFrameB.p = { 0.0f, m_doorHalfHeight, depths[i] };
|
||||
jointDef.enableMotor = true;
|
||||
jointDef.maxMotorTorque = 50.0f;
|
||||
b3CreateRevoluteJoint( m_worldId, &jointDef );
|
||||
}
|
||||
|
||||
// Prismatic axis is frame A local x, so rotate it onto world y.
|
||||
b3Quat slideAxis = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3Vec3_axisY );
|
||||
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
|
||||
jointDef.base.bodyIdA = groundId;
|
||||
jointDef.base.bodyIdB = doorId;
|
||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, doorPosition );
|
||||
jointDef.base.localFrameA.q = slideAxis;
|
||||
jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f };
|
||||
jointDef.base.localFrameB.q = slideAxis;
|
||||
jointDef.maxMotorForce = 200.0f;
|
||||
jointDef.enableMotor = true;
|
||||
jointDef.base.collideConnected = true;
|
||||
b3CreatePrismaticJoint( m_worldId, &jointDef );
|
||||
}
|
||||
|
||||
// Ring of 16 teeth around the gear rim at depth zCenter, in the body frame. Box3D
|
||||
// has no rounded hulls, so each tooth tapers tangentially toward the tip to clear
|
||||
// the meshing teeth the way Box2D's rounded teeth do.
|
||||
void AddTeeth( b3BodyId bodyId, float centerRadius, float zCenter )
|
||||
{
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.1f;
|
||||
shapeDef.baseMaterial.customColor = b3_colorGray;
|
||||
|
||||
int count = 16;
|
||||
float deltaAngle = 2.0f * B3_PI / count;
|
||||
|
||||
float hx = m_toothHalfWidth; // radial half extent
|
||||
float hz = m_gearHalfDepth; // depth half extent
|
||||
float baseHalf = m_toothHalfHeight; // tangential half width at the base
|
||||
float tipHalf = m_toothHalfHeight - m_toothRadius; // narrower at the tip
|
||||
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
b3Quat q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, i * deltaAngle );
|
||||
b3Vec3 center = b3RotateVector( q, { centerRadius, 0.0f, 0.0f } );
|
||||
center.z = zCenter;
|
||||
|
||||
// Base at the inner radius, narrower tip at the outer radius.
|
||||
b3Vec3 local[8] = {
|
||||
{ -hx, -baseHalf, -hz }, { -hx, baseHalf, -hz }, { -hx, baseHalf, hz }, { -hx, -baseHalf, hz },
|
||||
{ hx, -tipHalf, -hz }, { hx, tipHalf, -hz }, { hx, tipHalf, hz }, { hx, -tipHalf, hz },
|
||||
};
|
||||
|
||||
b3Vec3 points[8];
|
||||
for ( int k = 0; k < 8; ++k )
|
||||
{
|
||||
points[k] = b3Add( center, b3RotateVector( q, local[k] ) );
|
||||
}
|
||||
|
||||
b3HullData* tooth = b3CreateHull( points, 8, 8 );
|
||||
b3CreateHullShape( bodyId, &shapeDef, tooth );
|
||||
b3DestroyHull( tooth );
|
||||
}
|
||||
}
|
||||
|
||||
void CreateDebris()
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.rollingResistance = 0.3f;
|
||||
|
||||
b3HexColor colors[5] = {
|
||||
b3_colorGray, b3_colorGainsboro, b3_colorLightGray, b3_colorLightSlateGray, b3_colorDarkGray,
|
||||
};
|
||||
|
||||
b3HullData* rockHull = b3CreateRock( m_rockRadius );
|
||||
|
||||
float x = -5.0f;
|
||||
int xCount = 12, yCount = 10;
|
||||
for ( int i = 0; i < xCount; ++i )
|
||||
{
|
||||
float y = 6.5f - 0.25f * i;
|
||||
for ( int j = 0; j < yCount; ++j )
|
||||
{
|
||||
// Spread the debris across the depth of the stairwell.
|
||||
bodyDef.position = { x, y, RandomFloatRange( -1.65f, 0.35f ) };
|
||||
bodyDef.rotation = RandomQuat();
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
shapeDef.baseMaterial.customColor = colors[(int)RandomIntRange( 0, 4 )];
|
||||
b3CreateHullShape( bodyId, &shapeDef, rockHull );
|
||||
|
||||
y += 0.2f;
|
||||
}
|
||||
x += 0.3f;
|
||||
}
|
||||
|
||||
b3DestroyHull( rockHull );
|
||||
}
|
||||
|
||||
void SetMotorSpeed( float speed )
|
||||
{
|
||||
b3RevoluteJoint_SetMotorSpeed( m_driverId, speed );
|
||||
b3Joint_WakeBodies( m_driverId );
|
||||
}
|
||||
|
||||
bool DrawControls() override
|
||||
{
|
||||
if ( ImGui::Checkbox( "Motor", &m_enableMotor ) )
|
||||
{
|
||||
b3RevoluteJoint_EnableMotor( m_driverId, m_enableMotor );
|
||||
b3Joint_WakeBodies( m_driverId );
|
||||
}
|
||||
|
||||
ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() );
|
||||
|
||||
if ( ImGui::SliderFloat( "Max Torque", &m_motorTorque, 0.0f, 100000.0f, "%.0f" ) )
|
||||
{
|
||||
b3RevoluteJoint_SetMaxMotorTorque( m_driverId, m_motorTorque );
|
||||
b3Joint_WakeBodies( m_driverId );
|
||||
}
|
||||
|
||||
if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -0.3f, 0.3f, "%.2f" ) )
|
||||
{
|
||||
SetMotorSpeed( m_motorSpeed );
|
||||
}
|
||||
|
||||
ImGui::PopItemWidth();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new GearLift( context );
|
||||
}
|
||||
|
||||
static constexpr float m_gearRadius = 1.0f;
|
||||
static constexpr float m_gearHalfDepth = 0.125f; // 25 cm gear depth
|
||||
static constexpr float m_gearZ = 1.5f; // near and far gear planes
|
||||
static constexpr float m_axleRadius = 0.2f;
|
||||
static constexpr float m_toothHalfWidth = 0.11f;
|
||||
static constexpr float m_toothHalfHeight = 0.09f;
|
||||
static constexpr float m_toothRadius = 0.03f;
|
||||
static constexpr float m_linkHalfLength = 0.07f;
|
||||
static constexpr float m_linkRadius = 0.05f;
|
||||
static constexpr int m_linkCount = 40;
|
||||
static constexpr float m_doorHalfHeight = 1.5f;
|
||||
static constexpr float m_doorHalfDepth = 1.95f;
|
||||
static constexpr int m_gearSides = 24;
|
||||
static constexpr int m_axleSides = 12;
|
||||
static constexpr float m_rockRadius = 0.3f;
|
||||
|
||||
b3MeshData* m_mesh;
|
||||
b3JointId m_driverId;
|
||||
float m_motorTorque;
|
||||
float m_motorSpeed;
|
||||
bool m_enableMotor;
|
||||
};
|
||||
|
||||
static int sampleGearLift = RegisterSample( "Joints", "Gear Lift", GearLift::Create );
|
||||
|
|
|
|||
Loading…
Reference in a new issue