#include "vg_m.h"
-enum contact_type
-{
- k_contact_type_default,
- k_contact_type_disabled,
- k_contact_type_edge
-};
-
-/*
- * -----------------------------------------------------------------------------
- * Closest point functions
- * -----------------------------------------------------------------------------
- */
-
-/*
- * These closest point tests were learned from Real-Time Collision Detection by
- * Christer Ericson
- */
-VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
- float *s, float *t, v3f c1, v3f c2)
-{
- v3f d1,d2,r;
- v3_sub( q1, p1, d1 );
- v3_sub( q2, p2, d2 );
- v3_sub( p1, p2, r );
-
- float a = v3_length2( d1 ),
- e = v3_length2( d2 ),
- f = v3_dot( d2, r );
-
- const float kEpsilon = 0.0001f;
-
- if( a <= kEpsilon && e <= kEpsilon )
- {
- *s = 0.0f;
- *t = 0.0f;
- v3_copy( p1, c1 );
- v3_copy( p2, c2 );
-
- v3f v0;
- v3_sub( c1, c2, v0 );
-
- return v3_length2( v0 );
- }
-
- if( a<= kEpsilon )
- {
- *s = 0.0f;
- *t = vg_clampf( f / e, 0.0f, 1.0f );
- }
- else
- {
- float c = v3_dot( d1, r );
- if( e <= kEpsilon )
- {
- *t = 0.0f;
- *s = vg_clampf( -c / a, 0.0f, 1.0f );
- }
- else
- {
- float b = v3_dot(d1,d2),
- d = a*e-b*b;
-
- if( d != 0.0f )
- {
- *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
- }
- else
- {
- *s = 0.0f;
- }
-
- *t = (b*(*s)+f) / e;
-
- if( *t < 0.0f )
- {
- *t = 0.0f;
- *s = vg_clampf( -c / a, 0.0f, 1.0f );
- }
- else if( *t > 1.0f )
- {
- *t = 1.0f;
- *s = vg_clampf((b-c)/a,0.0f,1.0f);
- }
- }
- }
-
- v3_muladds( p1, d1, *s, c1 );
- v3_muladds( p2, d2, *t, c2 );
-
- v3f v0;
- v3_sub( c1, c2, v0 );
- return v3_length2( v0 );
-}
-
-VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
-{
- v3_maxv( p, box[0], dest );
- v3_minv( dest, box[1], dest );
-}
-
-VG_STATIC void closest_point_obb( v3f p, boxf box,
- m4x3f mtx, m4x3f inv_mtx, v3f dest )
-{
- v3f local;
- m4x3_mulv( inv_mtx, p, local );
- closest_point_aabb( local, box, local );
- m4x3_mulv( mtx, local, dest );
-}
-
-VG_STATIC float closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
-{
- v3f v0, v1;
- v3_sub( b, a, v0 );
- v3_sub( point, a, v1 );
-
- float t = v3_dot( v1, v0 ) / v3_length2(v0);
- t = vg_clampf(t,0.0f,1.0f);
- v3_muladds( a, v0, t, dest );
- return t;
-}
-
-VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
-{
- v3f ab, ac, ap;
- float d1, d2;
-
- /* Region outside A */
- v3_sub( tri[1], tri[0], ab );
- v3_sub( tri[2], tri[0], ac );
- v3_sub( p, tri[0], ap );
-
- d1 = v3_dot(ab,ap);
- d2 = v3_dot(ac,ap);
- if( d1 <= 0.0f && d2 <= 0.0f )
- {
- v3_copy( tri[0], dest );
- v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
- return;
- }
-
- /* Region outside B */
- v3f bp;
- float d3, d4;
-
- v3_sub( p, tri[1], bp );
- d3 = v3_dot( ab, bp );
- d4 = v3_dot( ac, bp );
-
- if( d3 >= 0.0f && d4 <= d3 )
- {
- v3_copy( tri[1], dest );
- v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
- return;
- }
-
- /* Edge region of AB */
- float vc = d1*d4 - d3*d2;
- if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
- {
- float v = d1 / (d1-d3);
- v3_muladds( tri[0], ab, v, dest );
- v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
- return;
- }
-
- /* Region outside C */
- v3f cp;
- float d5, d6;
- v3_sub( p, tri[2], cp );
- d5 = v3_dot(ab, cp);
- d6 = v3_dot(ac, cp);
-
- if( d6 >= 0.0f && d5 <= d6 )
- {
- v3_copy( tri[2], dest );
- v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
- return;
- }
-
- /* Region of AC */
- float vb = d5*d2 - d1*d6;
- if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
- {
- float w = d2 / (d2-d6);
- v3_muladds( tri[0], ac, w, dest );
- v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
- return;
- }
-
- /* Region of BC */
- float va = d3*d6 - d5*d4;
- if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
- {
- float w = (d4-d3) / ((d4-d3) + (d5-d6));
- v3f bc;
- v3_sub( tri[2], tri[1], bc );
- v3_muladds( tri[1], bc, w, dest );
- v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
- return;
- }
-
- /* P inside region, Q via barycentric coordinates uvw */
- float d = 1.0f/(va+vb+vc),
- v = vb*d,
- w = vc*d;
-
- v3_muladds( tri[0], ab, v, dest );
- v3_muladds( dest, ac, w, dest );
-}
-
-VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
-{
- v3f ab, ac, ap;
- float d1, d2;
-
- /* Region outside A */
- v3_sub( tri[1], tri[0], ab );
- v3_sub( tri[2], tri[0], ac );
- v3_sub( p, tri[0], ap );
-
- d1 = v3_dot(ab,ap);
- d2 = v3_dot(ac,ap);
- if( d1 <= 0.0f && d2 <= 0.0f )
- {
- v3_copy( tri[0], dest );
- return k_contact_type_default;
- }
-
- /* Region outside B */
- v3f bp;
- float d3, d4;
-
- v3_sub( p, tri[1], bp );
- d3 = v3_dot( ab, bp );
- d4 = v3_dot( ac, bp );
-
- if( d3 >= 0.0f && d4 <= d3 )
- {
- v3_copy( tri[1], dest );
- return k_contact_type_edge;
- }
-
- /* Edge region of AB */
- float vc = d1*d4 - d3*d2;
- if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
- {
- float v = d1 / (d1-d3);
- v3_muladds( tri[0], ab, v, dest );
- return k_contact_type_edge;
- }
-
- /* Region outside C */
- v3f cp;
- float d5, d6;
- v3_sub( p, tri[2], cp );
- d5 = v3_dot(ab, cp);
- d6 = v3_dot(ac, cp);
-
- if( d6 >= 0.0f && d5 <= d6 )
- {
- v3_copy( tri[2], dest );
- return k_contact_type_edge;
- }
-
- /* Region of AC */
- float vb = d5*d2 - d1*d6;
- if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
- {
- float w = d2 / (d2-d6);
- v3_muladds( tri[0], ac, w, dest );
- return k_contact_type_edge;
- }
-
- /* Region of BC */
- float va = d3*d6 - d5*d4;
- if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
- {
- float w = (d4-d3) / ((d4-d3) + (d5-d6));
- v3f bc;
- v3_sub( tri[2], tri[1], bc );
- v3_muladds( tri[1], bc, w, dest );
- return k_contact_type_edge;
- }
-
- /* P inside region, Q via barycentric coordinates uvw */
- float d = 1.0f/(va+vb+vc),
- v = vb*d,
- w = vc*d;
-
- v3_muladds( tri[0], ab, v, dest );
- v3_muladds( dest, ac, w, dest );
-
- return k_contact_type_default;
-}
#endif /* DISTQ_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff