Minkowski Collision Detection (Minkowski Difference)

// Transform for each chape
struct Transforms {
    matrixA : mat4x4<f32>,
    matrixB : mat4x4<f32>,
};

// Results we read back from the calculation
struct Results {
    data0: vec4<f32>,
    outContactPoint     : vec4<f32>,
    outContactNormal    : vec4<f32>,
    outPenetrationDepth : vec4<f32>,
    outContactPointB    : vec4<f32>, // 4

    X : vec4<f32>, // 5 ->
    Y : vec4<f32>,
    Z : vec4<f32>,
    W : vec4<f32>,

    A : vec4<f32>, //
    B : vec4<f32>,
    C : vec4<f32>,
    D : vec4<f32>,
}

// binding information for the in/out from the comopute shader
@group(0) @binding(0) var<uniform> transforms : Transforms;
@group(0) @binding(1) var<storage, read_write> shapeA   : array< f32 >;  // use f32 to avoid any alignment issues
@group(0) @binding(2) var<storage, read_write> shapeB   : array< f32 >;  // tighly packed to 4 byte
@group(0) @binding(3) var<storage, read_write> results  : Results;
@group(0) @binding(4) var<storage, read_write> tmpg     : f32;

/*
Instead of passing an array of 'vec3<f32>' - we pass a single array of floats - every 3 floats is a vertex position.

This was a workaround for the alignment - compute shader likes to pad structures to 16 bytes (4 floats).
*/
fn GetVertex( shapeId:u32, indx:u32 ) -> vec3<f32>
{
    var p : vec3<f32>  = vec3<f32>(0);
    if ( shapeId == 0 ) {
        p.x=shapeA[indx*3+0]; p.y=shapeA[indx*3+1]; p.z=shapeA[indx*3+2];
        p = (transforms.matrixA * vec4<f32>(p,1.0) ).xyz;
    };
    if ( shapeId == 1 ) {
        p.x=shapeB[indx*3+0]; p.y=shapeB[indx*3+1]; p.z=shapeB[indx*3+2];
        p = (transforms.matrixB * vec4<f32>(p,1.0) ).xyz;
    };
    return p;
}

struct Centroids {
    A: vec3<f32>,
    B: vec3<f32>
}

fn GetCentroids() -> Centroids {

    var centroids: Centroids;

    centroids.A = vec3<f32>(0);
    for (var i:u32=0; i< u32(arrayLength(&shapeA)/3);  i++)
    {
        let pA = GetVertex( 0, i );
        centroids.A += pA;
    }
    centroids.A  = centroids.A * 1.0/f32( arrayLength(&shapeA)/3 );

    centroids.B = vec3<f32>(0);
    for (var k:u32=0; k< u32(arrayLength(&shapeB)/3);  k++)
    {
        let pB = GetVertex(1,k);
        centroids.B += pB;
    }
    centroids.B  = centroids.B * 1.0/f32( arrayLength(&shapeB)/3 );

    return centroids;
}

struct MeshSupportPoint {
    p: vec3<f32>,
    i: u32
}

fn GetMeshSupportVertex( shapeId:u32, n:vec3<f32>) -> MeshSupportPoint
{
    var numShapePoints:u32 = 0;
    if ( shapeId==0 ) { numShapePoints=arrayLength(&shapeA)/3; };
    if ( shapeId==1 ) { numShapePoints=arrayLength(&shapeB)/3; };

    var supportPoint : vec3<f32>;
    var mmax = -1000000.0;
    var ip:u32 = 0;
    for (var i:u32=0; i<numShapePoints; i++)
    {
        var p : vec3<f32> = GetVertex(shapeId, i);
        var val = dot( p, n );

        if ( val > mmax )
        {
            supportPoint = p;
            ip = i/3;
            mmax = val;
        }
    }
    return MeshSupportPoint(supportPoint, ip);
}

struct SupportPoint {
    p: vec3<f32>,
    n: vec3<f32>,
    a: MeshSupportPoint,
    b: MeshSupportPoint
}

fn GetSupportVertex( n:vec3<f32>) -> SupportPoint
{
    var v0 = GetMeshSupportVertex( 0,  n );
    var v1 = GetMeshSupportVertex( 1, -n );
    return SupportPoint(v0.p - v1.p, n, v0, v1);
}// End GetSupportVertex(..)

fn MapPointOrigin(A:vec3<f32>,   B:vec3<f32>,  C:vec3<f32>,
                  tA:vec3<f32>, tB:vec3<f32>, tC:vec3<f32> ) -> vec3<f32>
{
    let origin = vec3<f32>(0,0,0);
    let n = normalize( cross( A-B, A-C ) );
    let p = origin + n * dot( n, A - origin );

    var tt = length( cross(p-C, A-C) );
    var tb = length( cross(B-C, A-C) );

    var st = length( cross(p-C, B-C) );
    var sb = length( cross(A-C, B-C) );

    var t = tt/tb;
    t = clamp(t, 0, 1 );

    var s = st/sb;
    s = clamp(s, 0, 1 );

    return tC + (tA-tC)*s + (tB-tC)*t;

}// End MapPointOrigin(..)

/* -----------------------------------------------------------------------------

Compute shader entry point

----------------------------------------------------------------------------- */
@compute @workgroup_size(1, 1)
fn main(@builtin(global_invocation_id) globalId      : vec3<u32>,
        @builtin(local_invocation_id)  localId       : vec3<u32>,
        @builtin(workgroup_id)         workgroupId   : vec3<u32>,
        @builtin(num_workgroups)       workgroupSize : vec3<u32>
        )
{
    // These are the objectives - what we want to find! - fill them with debug values
    var outPenetrationDepth : f32       = 1.23456;
    var outContactPoint     : vec3<f32> = vec3<f32>(1.23456);
    var outContactNormal    : vec3<f32> = vec3<f32>(1.23456);

    let centroids = GetCentroids();

    var origin = vec3<f32>(0,0,0); // our target!

    var v0 = SupportPoint(centroids.A - centroids.B,
                          vec3<f32>(0),
                          MeshSupportPoint(vec3<f32>(0),0),
                          MeshSupportPoint(vec3<f32>(0),0)  );

    var v1 = GetSupportVertex( origin - v0.p );
    var v2 = GetSupportVertex( cross(origin-v0.p, origin-v1.p) );
    var dir3 = cross(v1.p-v0.p, v2.p-v0.p);

    if ( dot( dir3, v0.p - origin ) > 0.0 ) { dir3 = -dir3; }
    var v3 = GetSupportVertex( dir3);

    // v0, v1, v2 and v3 form a tetrahedron - however, we need to do
    // a bit of looping around to 'ensure' the tetrahedron encloses the origin
    for (var nn=0; nn<100; nn++)
    {
        if ( dot( cross(v1.p-v0.p, v3.p-v0.p), v0.p - origin ) < 0 )
        {
            v2 = v3;
            v3 = GetSupportVertex( cross(v1.p-v0.p, v3.p-v0.p) );
            continue;
        }

        if ( dot( cross(v3.p-v0.p,v2.p-v0.p), v0.p - origin ) < 0.0 )
        {
            v1 = v3;
            v3 = GetSupportVertex( cross(v3.p-v0.p, v2.p-v0.p));
            continue;
        }
        break;
    }

    // Use for with uppper limit instead of a while - avoid chance of an ininit loop
    // reduce to control fidelity/performance factors
    for (var nn=0; nn<10; nn++)
    {
        // Compute normal of the wedge face
        var v4 = GetSupportVertex( cross(v2.p - v1.p, v3.p - v1.p) );

        // Is our new point already on the plane of our triangle,
        // if so, we're already as close as we can get to the target
        var delta = dot( (v4.p - v3.p), v4.n );
        if ( abs(delta) < 0.01 )
        {
            var n = normalize( v4.n );
            // Compute distance from origin to wedge face
            outPenetrationDepth = dot(n, v1.p-origin);

            // we're inside the wedge and have a collision
            // break and return closest point, normal and peneration/distance
            break;
        }

        // We'll create three baby tetrahedrons and decide
        // which one will replace the current tetrahedron
        // v1v2 - v2v3 - v3v1 - current tetrahedron edges
        // leads to 3 new tetrahedrons:
        // v1v2v4
        // v2v3v4
        // v3v1v4
        // Use for with uppper limit instead of a while - avoid chance of an ininit loop
        //while ( true )
        for (var gg=0; gg<100; gg++)
        {
            var na = -cross( v2.p-v0.p, v4.p-v0.p ); // Normal to the triangle v2v4v0
            var nb = -cross( v4.p-v0.p, v1.p-v0.p ); // Normal to the triangle v4v1v0
            if ( dot( na, v0.p-origin ) > 0 &&
                 dot( nb, v0.p-origin ) > 0 )
            {
                // Inside this tetrahedron
                v3   = v4;
                break;
            }

            na = -cross( v3.p-v0.p, v4.p-v0.p ); // Normal to the triangle v3v4v0
            nb = -cross( v4.p-v0.p, v2.p-v0.p ); // Normal to the triangle v4v2v0
            if ( dot( na, v0.p-origin ) > 0 &&
                 dot( nb, v0.p-origin ) > 0 )
            {
                // Inside this tetrahedron
                v1 = v4;
                break;
            }

            na = -cross( v1.p-v0.p, v4.p-v0.p ); // Normal to the triangle v1v4v0
            nb = -cross( v4.p-v0.p, v3.p-v0.p ); // Normal to the triangle v4v3v0
            if ( dot( na, v0.p-origin ) > 0 &&
                 dot( nb, v0.p-origin ) > 0 )
            {
                // Inside this tetrahedron
                v2 = v4;
                break;
            }

            // Should never get here - as it must be in
            // one of the children!
            // assert - fix algorithm!!
            break;
        }

    }// End loop

    // Barycentric coordinates to map from the minkowski
    // difference onto the original shape
    outContactPoint = MapPointOrigin( v1.p, v2.p, v3.p,
                                      v1.a.p, v2.a.p, v3.a.p );

    var outContactPointB = MapPointOrigin( v1.p,   v2.p,   v3.p,
                                           v1.b.p, v2.b.p, v3.b.p );


    outContactNormal = cross( v1.p-v2.p, v1.p-v3.p );
    outContactNormal = normalize( outContactNormal );

    // store extra debug information - triangles on each shape
    results.A     = vec4<f32>( v1.a.p, 0.0);
    results.B     = vec4<f32>( v2.a.p, 0.0);
    results.C     = vec4<f32>( v3.a.p, 0.0);

    results.X     = vec4<f32>( v1.b.p, 0.0);
    results.Y     = vec4<f32>( v2.b.p, 0.0);
    results.Z     = vec4<f32>( v3.b.p, 0.0);

    results.outContactPoint     = vec4<f32>( outContactPoint, 0.0);
    results.outContactNormal    = vec4<f32>( outContactNormal, 0.0);

    // outPenetrationDepth < 0 - hit/otherwise no hit
    results.outPenetrationDepth = vec4<f32>( outPenetrationDepth, 0, 0, 0 );

    results.outContactPointB    = vec4<f32>( outContactPointB, 0 );

    results.data0.x = f32( arrayLength(&shapeA) );
    results.data0.y = f32( arrayLength(&shapeB) );
}

How it works

Tetrahedrons and Bookkeeping

Implementation Example

Resources & Links