_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
Vector3 axis = p_axis;
if (Math::abs(axis.x) < CMP_EPSILON &&
Math::abs(axis.y) < CMP_EPSILON &&
Math::abs(axis.z) < CMP_EPSILON) {
// strange case, try an upwards separator
axis = Vector3(0.0, 1.0, 0.0);
}
real_t min_A, max_A, min_B, max_B;
shape_A->project_range(axis, *transform_A, min_A, max_A);
shape_B->project_range(axis, *transform_B, min_B, max_B);
if (withMargin) {
min_A -= margin_A;
max_A += margin_A;
min_B -= margin_B;
max_B += margin_B;
}
min_B -= (max_A - min_A) * 0.5;
max_B += (max_A - min_A) * 0.5;
real_t dmin = min_B - (min_A + max_A) * 0.5;
real_t dmax = max_B - (min_A + max_A) * 0.5;
if (dmin > 0.0 || dmax < 0.0) {
separator_axis = axis;
return false; // doesn't contain 0
}
//use the smallest depth
dmin = Math::abs(dmin);
if (dmax < dmin) {
if (dmax < best_depth) {
best_depth = dmax;
best_axis = axis;
}
} else {
if (dmin < best_depth) {
best_depth = dmin;
best_axis = -axis; // keep it as A axis
}
}
return true;
}
_FORCE_INLINE_ bool test_axis(const Vector2& p_axis) {
Vector2 axis=p_axis;
if ( Math::abs(axis.x)<CMP_EPSILON &&
Math::abs(axis.y)<CMP_EPSILON) {
// strange case, try an upwards separator
axis=Vector2(0.0,1.0);
}
real_t min_A,max_A,min_B,max_B;
if (castA)
shape_A->project_range_cast(motion_A,axis,*transform_A,min_A,max_A);
else
shape_A->project_range(axis,*transform_A,min_A,max_A);
if (castB)
shape_B->project_range_cast(motion_B,axis,*transform_B,min_B,max_B);
else
shape_B->project_range(axis,*transform_B,min_B,max_B);
if (withMargin) {
min_A-=margin_A;
max_A+=margin_A;
min_B-=margin_B;
max_B+=margin_B;
}
min_B -= ( max_A - min_A ) * 0.5;
max_B += ( max_A - min_A ) * 0.5;
real_t dmin = min_B - ( min_A + max_A ) * 0.5;
real_t dmax = max_B - ( min_A + max_A ) * 0.5;
if (dmin > 0.0 || dmax < 0.0) {
if (callback && callback->sep_axis)
*callback->sep_axis=axis;
#ifdef DEBUG_ENABLED
best_axis_count++;
#endif
return false; // doesn't contain 0
}
//use the smallest depth
dmin = Math::abs(dmin);
if ( dmax < dmin ) {
if ( dmax < best_depth ) {
best_depth=dmax;
best_axis=axis;
#ifdef DEBUG_ENABLED
best_axis_index=best_axis_count;
#endif
}
} else {
if ( dmin < best_depth ) {
best_depth=dmin;
best_axis=-axis; // keep it as A axis
#ifdef DEBUG_ENABLED
best_axis_index=best_axis_count;
#endif
}
}
// print_line("test axis: "+p_axis+" depth: "+rtos(best_depth));
#ifdef DEBUG_ENABLED
best_axis_count++;
#endif
return true;
}