- b3MeshDef gets stride and clockwise option - Added b3Joint_IsAwake - Added b3Body_TimeOfImpactMover - #116 - Smoother mover collision: #141 - Shape casts now ignore backside collision on mesh and height-field triangles, same as ray casts.
385 lines
11 KiB
C
385 lines
11 KiB
C
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "math_internal.h"
|
|
#include "shape.h"
|
|
|
|
#include "box3d/base.h"
|
|
#include "box3d/collision.h"
|
|
#include "box3d/constants.h"
|
|
|
|
b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density )
|
|
{
|
|
b3Vec3 c1 = shape->center1;
|
|
b3Vec3 c2 = shape->center2;
|
|
float r = shape->radius;
|
|
|
|
// Cylinder
|
|
float cylinderHeight = b3Distance( c1, c2 );
|
|
float cylinderVolume = B3_PI * r * r * cylinderHeight;
|
|
float cylinderMass = cylinderVolume * density;
|
|
|
|
// Sphere
|
|
float sphereVolume = ( 4.0f / 3.0f ) * B3_PI * r * r * r;
|
|
float sphereMass = sphereVolume * density;
|
|
|
|
// Local accumulated inertia
|
|
b3Matrix3 inertia = b3AddMM( b3CylinderInertia( cylinderMass, r, cylinderHeight ), b3SphereInertia( sphereMass, r ) );
|
|
|
|
float steiner = 0.125f * sphereMass * ( 3.0f * r + 2.0f * cylinderHeight ) * cylinderHeight;
|
|
inertia.cx.x += steiner;
|
|
inertia.cz.z += steiner;
|
|
|
|
// Align capsule axis with chosen up-axis
|
|
b3Matrix3 rotation = b3Mat3_identity;
|
|
if ( cylinderHeight * cylinderHeight > 1000.0f * FLT_MIN )
|
|
{
|
|
b3Vec3 direction = b3Normalize( b3Sub( c2, c1 ) );
|
|
b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, direction );
|
|
rotation = b3MakeMatrixFromQuat( q );
|
|
}
|
|
|
|
float mass = sphereMass + cylinderMass;
|
|
b3Vec3 center = b3MulSV( 0.5f, b3Add( c1, c2 ) );
|
|
|
|
b3MassData out;
|
|
out.mass = mass;
|
|
out.center = center;
|
|
|
|
// Rotate the central inertia into the shape frame
|
|
out.inertia = b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) );
|
|
|
|
return out;
|
|
}
|
|
|
|
b3AABB b3ComputeCapsuleAABB( const b3Capsule* shape, b3Transform transform )
|
|
{
|
|
float r = shape->radius;
|
|
|
|
b3Vec3 center1 = b3TransformPoint( transform, shape->center1 );
|
|
b3Vec3 center2 = b3TransformPoint( transform, shape->center2 );
|
|
b3Vec3 extent = { r, r, r };
|
|
|
|
b3AABB aabb;
|
|
aabb.lowerBound = b3Sub( b3Min( center1, center2 ), extent );
|
|
aabb.upperBound = b3Add( b3Max( center1, center2 ), extent );
|
|
return aabb;
|
|
}
|
|
|
|
b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 )
|
|
{
|
|
b3Vec3 r = { shape->radius, shape->radius, shape->radius };
|
|
b3Vec3 a = b3TransformPoint( xf1, shape->center1 );
|
|
b3Vec3 b = b3TransformPoint( xf1, shape->center2 );
|
|
b3Vec3 c = b3TransformPoint( xf2, shape->center1 );
|
|
b3Vec3 d = b3TransformPoint( xf2, shape->center2 );
|
|
|
|
b3AABB aabb = {
|
|
.lowerBound = b3Sub( b3Min( b3Min( a, b ), b3Min( c, d ) ), r ),
|
|
.upperBound = b3Add( b3Max( b3Max( a, b ), b3Max( c, d ) ), r ),
|
|
};
|
|
return aabb;
|
|
}
|
|
|
|
bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy )
|
|
{
|
|
b3DistanceInput input;
|
|
input.proxyA = (b3ShapeProxy){ &shape->center1, 2, shape->radius };
|
|
input.proxyB = *proxy;
|
|
input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );
|
|
input.useRadii = true;
|
|
|
|
b3SimplexCache cache = { 0 };
|
|
b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );
|
|
return output.distance < B3_OVERLAP_SLOP;
|
|
}
|
|
|
|
// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019
|
|
// http://www.codercorner.com/blog/?p=321
|
|
b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input )
|
|
{
|
|
B3_ASSERT( b3IsValidRay( input ) );
|
|
|
|
b3Vec3 c1 = shape->center1;
|
|
b3Vec3 c2 = shape->center2;
|
|
float r = shape->radius;
|
|
|
|
// Initialize result structure
|
|
b3CastOutput output = { 0 };
|
|
|
|
b3Vec3 d = b3Sub( c2, c1 );
|
|
|
|
// Fall back to sphere if the capsule is short
|
|
float tol = 0.01f * B3_LINEAR_SLOP;
|
|
float lengthSquared = b3LengthSquared( d );
|
|
if ( lengthSquared < tol * tol )
|
|
{
|
|
b3Vec3 sphereCenter = b3MulSV( 0.5f, b3Add( shape->center1, shape->center2 ) );
|
|
b3Sphere sphere = { sphereCenter, shape->radius };
|
|
return b3RayCastSphere( &sphere, input );
|
|
}
|
|
|
|
// Vector from first center to ray origin.
|
|
b3Vec3 s = b3Sub( input->origin, c1 );
|
|
|
|
// Capsule axis
|
|
float length = sqrtf( lengthSquared );
|
|
b3Vec3 axis = b3MulSV( 1.0f / length, d );
|
|
|
|
// Project ray origin onto capsule axis.
|
|
float u = b3Dot( s, axis );
|
|
|
|
// Closest point on infinite capsule axis, relative to c1.
|
|
b3Vec3 c = b3MulSV( u, axis );
|
|
|
|
// Vector from closest point to ray origin
|
|
b3Vec3 sc = b3Sub( s, c );
|
|
|
|
// Squared distance from ray origin to capsule axis
|
|
float sc2 = b3LengthSquared( sc );
|
|
|
|
// Is the ray origin within the infinite cylinder along the capsule axis?
|
|
if ( sc2 < r * r )
|
|
{
|
|
// Clamped barycentric coordinate of ray origin projected onto capsule axis.
|
|
float uClamped = b3ClampFloat( u, 0.0f, length );
|
|
|
|
// The closest point on the bounded capsule segment, relative to c1.
|
|
b3Vec3 cp = b3MulSV( uClamped, axis );
|
|
|
|
// Vector from ray origin to closest point on segment.
|
|
b3Vec3 scp = b3Sub( s, cp );
|
|
|
|
// Squared distance of ray origin from capsule segment.
|
|
float scp2 = b3LengthSquared( scp );
|
|
|
|
// Is the ray origin within the capsule?
|
|
if ( scp2 < r * r )
|
|
{
|
|
output.hit = true;
|
|
output.point = input->origin;
|
|
return output;
|
|
}
|
|
|
|
// The ray can hit an endcap.
|
|
b3Sphere sphere = {
|
|
.center = b3Add( c1, cp ),
|
|
.radius = r,
|
|
};
|
|
|
|
return b3RayCastSphere( &sphere, input );
|
|
}
|
|
|
|
// Ray axis. A zero length ray reaching here starts outside the capsule, so it misses.
|
|
// Same zero length convention as b3RayCastSphere.
|
|
b3Vec3 dr = input->translation;
|
|
float rayLength;
|
|
b3Vec3 rayAxis = b3GetLengthAndNormalize( &rayLength, dr );
|
|
if ( rayLength == 0.0f )
|
|
{
|
|
return output;
|
|
}
|
|
|
|
// Barycentric coordinate of ray end point.
|
|
float v = u + input->maxFraction * b3Dot( dr, axis );
|
|
|
|
// Early out: does the projected ray fall outside the capsule?
|
|
if ( ( u < -r && v < -r ) || ( length + r < u && length + r < v ) )
|
|
{
|
|
return output;
|
|
}
|
|
|
|
// Compute the closest point between the ray segment and the capsule segment.
|
|
// See Real-Time Collision Detection, section 5.1.9
|
|
|
|
// Closest point on capsule : a1 = segment unit axis, t1 = unknown fraction
|
|
// p1 = t1 * a1
|
|
|
|
// Closet point on ray : a2 = ray unit axis, t2 = unknown fraction
|
|
// p2 = s + t2 * a2
|
|
|
|
// Closest point perpendicularity conditions.
|
|
// dot(p2 - p1, a1) = 0
|
|
// dot(p2 - p1, a2) = 0
|
|
|
|
// Expand
|
|
// dot(t1 * a1 - s - t2 * a2, a1) = 0
|
|
// dot(t1 * a1 - s - t2 * a2, a2) = 0
|
|
|
|
// Expand
|
|
// t1 - dot(s, a1) - t2 * dot(a1, a2) = 0
|
|
// t1 * dot(a1, a2) - dot(s, a2) - t2 = 0
|
|
|
|
// Group : a12 = dot(a1, a2), sa1 = dot(s, a1), sa2 = dot(s, a2)
|
|
// t1 - a12 * t2 = sa1
|
|
// a12 * t1 - t2 = sa2
|
|
|
|
// Solve
|
|
// https://en.wikipedia.org/wiki/Cramer%27s_rule
|
|
// I've flipped the signs of the numerator and denominator to give a positive determinant.
|
|
// det = 1 - a12 * a12
|
|
// t1 = (sa1 - a12 * sa2) / det
|
|
// t2 = (a12 * sa1 - sa2) / det
|
|
|
|
b3Vec3 a1 = axis;
|
|
b3Vec3 a2 = rayAxis;
|
|
float a12 = b3Dot( a1, a2 );
|
|
|
|
// Ray distance to the near intersection with the infinite cylinder. Length units.
|
|
float tr;
|
|
|
|
float det = 1.0f - a12 * a12;
|
|
if ( det < FLT_EPSILON )
|
|
{
|
|
// Solve the 2D problem of ray versus circle starting at the ray origin, where the circle is
|
|
// the axial view of the infinite capsule cylinder. This works well when the ray origin is
|
|
// not too far from the capsule axis.
|
|
|
|
// Instead of a cross product, subtract the parallel part to get a perpendicular vector. Non-dimensional.
|
|
b3Vec3 perp = b3MulSub( a2, a12, a1 );
|
|
float perp2 = b3LengthSquared( perp );
|
|
|
|
// Project to origin to infinite capsule axis vector onto the perpendicular vector. beta has length units.
|
|
float beta = b3Dot( sc, perp );
|
|
|
|
// Setup quadratic root finder.
|
|
float gamma = sc2 - r * r;
|
|
|
|
// Discriminant
|
|
float disc = beta * beta - perp2 * gamma;
|
|
|
|
// Casting away from the axis, or the perpendicular gap never closes to the radius.
|
|
if ( beta >= 0.0f || disc < 0.0f )
|
|
{
|
|
return output;
|
|
}
|
|
|
|
// Quadratic near root. Expressed in an alternate form to avoid the (-beta - sqrt) cancellation as
|
|
// the ray nears parallel.
|
|
tr = gamma / ( -beta + sqrtf( disc ) );
|
|
}
|
|
else
|
|
{
|
|
// Ray and capsules axes are not parallel.
|
|
|
|
// Closest points between the infinite ray and the infinite capsule axis.
|
|
float invDet = 1.0f / det;
|
|
float sa1 = u;
|
|
float sa2 = b3Dot( s, a2 );
|
|
|
|
float t1 = ( sa1 - a12 * sa2 ) * invDet;
|
|
float t2 = ( a12 * sa1 - sa2 ) * invDet;
|
|
|
|
// Closest points
|
|
b3Vec3 p1 = b3MulSV( t1, a1 );
|
|
b3Vec3 p2 = b3MulAdd( s, t2, a2 );
|
|
|
|
// Vector from closest point on infinite capsule to infinite ray.
|
|
b3Vec3 g = b3Sub( p2, p1 );
|
|
|
|
float g2 = b3LengthSquared( g );
|
|
if ( g2 > r * r )
|
|
{
|
|
// Early out: closest point on infinite ray is outside infinite cylinder.
|
|
return output;
|
|
}
|
|
|
|
// Intersect the infinite ray with the infinite cylinder. Like ray versus sphere this is done
|
|
// relative to the closest point to avoid round-off errors. Length units, not a fraction.
|
|
// https://en.wikipedia.org/wiki/Line-cylinder_intersection
|
|
float h = sqrtf( ( r * r - g2 ) * invDet );
|
|
|
|
tr = t2 - h;
|
|
}
|
|
|
|
// Outside ray?
|
|
if ( tr < 0.0f || input->maxFraction * rayLength < tr )
|
|
{
|
|
return output;
|
|
}
|
|
|
|
// The corresponding distance on the capsule axis. Length units.
|
|
float tc = u + tr * a12;
|
|
|
|
// Outside c1 end?
|
|
if ( tc < 0.0f )
|
|
{
|
|
// Ray cast sphere 1.
|
|
b3Sphere sphere = {
|
|
.center = c1,
|
|
.radius = r,
|
|
};
|
|
|
|
return b3RayCastSphere( &sphere, input );
|
|
}
|
|
|
|
// Outside c2 end?
|
|
if (length < tc)
|
|
{
|
|
// Ray cast sphere 2.
|
|
b3Sphere sphere = {
|
|
.center = c2,
|
|
.radius = r,
|
|
};
|
|
|
|
return b3RayCastSphere( &sphere, input );
|
|
}
|
|
|
|
// Hit point on capsule side, relative to c1.
|
|
b3Vec3 p = b3MulAdd( s, tr, rayAxis );
|
|
|
|
// Hit normal.
|
|
b3Vec3 normal = b3MulSub( p, tc, axis );
|
|
normal = b3Normalize( normal );
|
|
|
|
output.point = b3Add( c1, p );
|
|
output.normal = normal;
|
|
output.fraction = b3ClampFloat( tr / rayLength, 0.0f, input->maxFraction );
|
|
output.hit = true;
|
|
return output;
|
|
}
|
|
|
|
b3CastOutput b3ShapeCastCapsule( const b3Capsule* capsule, const b3ShapeCastInput* input )
|
|
{
|
|
b3ShapeCastPairInput pairInput;
|
|
pairInput.proxyA = (b3ShapeProxy){ &capsule->center1, 2, capsule->radius };
|
|
pairInput.proxyB = input->proxy;
|
|
pairInput.transform = b3Transform_identity;
|
|
pairInput.translationB = input->translation;
|
|
pairInput.maxFraction = input->maxFraction;
|
|
pairInput.canEncroach = input->canEncroach;
|
|
|
|
b3CastOutput output = b3ShapeCast( &pairInput );
|
|
return output;
|
|
}
|
|
|
|
int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover )
|
|
{
|
|
float totalRadius = mover->radius + shape->radius;
|
|
|
|
b3SegmentDistanceResult approach = b3SegmentDistance( shape->center1, shape->center2, mover->center1, mover->center2 );
|
|
|
|
// The normal points from the shape toward the mover.
|
|
float distance;
|
|
b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( approach.point2, approach.point1 ) );
|
|
|
|
if ( distance > totalRadius )
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
float linearSlop = B3_LINEAR_SLOP;
|
|
if ( distance < linearSlop )
|
|
{
|
|
// Deep overlap: the core segments intersect. Pick an arbitrary direction perpendicular
|
|
// the to capsule axis.
|
|
float moverLength;
|
|
b3Vec3 moverAxis = b3GetLengthAndNormalize( &moverLength, b3Sub( mover->center2, mover->center1 ) );
|
|
normal = moverLength > linearSlop ? b3Perp( moverAxis ) : b3Vec3_axisY;
|
|
distance = 0.0f;
|
|
}
|
|
|
|
b3Plane plane = { normal, totalRadius - distance };
|
|
*result = (b3PlaneResult){ plane, approach.point1, 0, 0, 0 };
|
|
return 1;
|
|
}
|