8 k_contact_type_default
,
9 k_contact_type_disabled
,
14 * -----------------------------------------------------------------------------
15 * Closest point functions
16 * -----------------------------------------------------------------------------
20 * These closest point tests were learned from Real-Time Collision Detection by
23 VG_STATIC
float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
24 float *s
, float *t
, v3f c1
, v3f c2
)
31 float a
= v3_length2( d1
),
35 const float kEpsilon
= 0.0001f
;
37 if( a
<= kEpsilon
&& e
<= kEpsilon
)
47 return v3_length2( v0
);
53 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
57 float c
= v3_dot( d1
, r
);
61 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
65 float b
= v3_dot(d1
,d2
),
70 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
82 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
87 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
92 v3_muladds( p1
, d1
, *s
, c1
);
93 v3_muladds( p2
, d2
, *t
, c2
);
97 return v3_length2( v0
);
100 VG_STATIC
void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
102 v3_maxv( p
, box
[0], dest
);
103 v3_minv( dest
, box
[1], dest
);
106 VG_STATIC
void closest_point_obb( v3f p
, boxf box
,
107 m4x3f mtx
, m4x3f inv_mtx
, v3f dest
)
110 m4x3_mulv( inv_mtx
, p
, local
);
111 closest_point_aabb( local
, box
, local
);
112 m4x3_mulv( mtx
, local
, dest
);
115 VG_STATIC
float closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
119 v3_sub( point
, a
, v1
);
121 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
122 t
= vg_clampf(t
,0.0f
,1.0f
);
123 v3_muladds( a
, v0
, t
, dest
);
127 VG_STATIC
void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
132 /* Region outside A */
133 v3_sub( tri
[1], tri
[0], ab
);
134 v3_sub( tri
[2], tri
[0], ac
);
135 v3_sub( p
, tri
[0], ap
);
139 if( d1
<= 0.0f
&& d2
<= 0.0f
)
141 v3_copy( tri
[0], dest
);
142 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
146 /* Region outside B */
150 v3_sub( p
, tri
[1], bp
);
151 d3
= v3_dot( ab
, bp
);
152 d4
= v3_dot( ac
, bp
);
154 if( d3
>= 0.0f
&& d4
<= d3
)
156 v3_copy( tri
[1], dest
);
157 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
161 /* Edge region of AB */
162 float vc
= d1
*d4
- d3
*d2
;
163 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
165 float v
= d1
/ (d1
-d3
);
166 v3_muladds( tri
[0], ab
, v
, dest
);
167 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
171 /* Region outside C */
174 v3_sub( p
, tri
[2], cp
);
178 if( d6
>= 0.0f
&& d5
<= d6
)
180 v3_copy( tri
[2], dest
);
181 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
186 float vb
= d5
*d2
- d1
*d6
;
187 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
189 float w
= d2
/ (d2
-d6
);
190 v3_muladds( tri
[0], ac
, w
, dest
);
191 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
196 float va
= d3
*d6
- d5
*d4
;
197 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
199 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
201 v3_sub( tri
[2], tri
[1], bc
);
202 v3_muladds( tri
[1], bc
, w
, dest
);
203 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
207 /* P inside region, Q via barycentric coordinates uvw */
208 float d
= 1.0f
/(va
+vb
+vc
),
212 v3_muladds( tri
[0], ab
, v
, dest
);
213 v3_muladds( dest
, ac
, w
, dest
);
216 VG_STATIC
enum contact_type
closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
221 /* Region outside A */
222 v3_sub( tri
[1], tri
[0], ab
);
223 v3_sub( tri
[2], tri
[0], ac
);
224 v3_sub( p
, tri
[0], ap
);
228 if( d1
<= 0.0f
&& d2
<= 0.0f
)
230 v3_copy( tri
[0], dest
);
231 return k_contact_type_default
;
234 /* Region outside B */
238 v3_sub( p
, tri
[1], bp
);
239 d3
= v3_dot( ab
, bp
);
240 d4
= v3_dot( ac
, bp
);
242 if( d3
>= 0.0f
&& d4
<= d3
)
244 v3_copy( tri
[1], dest
);
245 return k_contact_type_edge
;
248 /* Edge region of AB */
249 float vc
= d1
*d4
- d3
*d2
;
250 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
252 float v
= d1
/ (d1
-d3
);
253 v3_muladds( tri
[0], ab
, v
, dest
);
254 return k_contact_type_edge
;
257 /* Region outside C */
260 v3_sub( p
, tri
[2], cp
);
264 if( d6
>= 0.0f
&& d5
<= d6
)
266 v3_copy( tri
[2], dest
);
267 return k_contact_type_edge
;
271 float vb
= d5
*d2
- d1
*d6
;
272 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
274 float w
= d2
/ (d2
-d6
);
275 v3_muladds( tri
[0], ac
, w
, dest
);
276 return k_contact_type_edge
;
280 float va
= d3
*d6
- d5
*d4
;
281 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
283 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
285 v3_sub( tri
[2], tri
[1], bc
);
286 v3_muladds( tri
[1], bc
, w
, dest
);
287 return k_contact_type_edge
;
290 /* P inside region, Q via barycentric coordinates uvw */
291 float d
= 1.0f
/(va
+vb
+vc
),
295 v3_muladds( tri
[0], ab
, v
, dest
);
296 v3_muladds( dest
, ac
, w
, dest
);
298 return k_contact_type_default
;