_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_ void generate_contacts() {
// nothing to do, don't generate
if (best_axis==Vector2(0.0,0.0))
return;
callback->collided=true;
if (!callback->callback)
return; //only collide, no callback
static const int max_supports=2;
Vector2 supports_A[max_supports];
int support_count_A;
if (castA) {
shape_A->get_supports_transformed_cast(motion_A,-best_axis,*transform_A,supports_A,support_count_A);
} else {
shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
for(int i=0;i<support_count_A;i++) {
supports_A[i] = transform_A->xform(supports_A[i]);
}
}
if (withMargin) {
for(int i=0;i<support_count_A;i++) {
supports_A[i]+=-best_axis*margin_A;
}
}
Vector2 supports_B[max_supports];
int support_count_B;
if (castB) {
shape_B->get_supports_transformed_cast(motion_B,best_axis,*transform_B,supports_B,support_count_B);
} else {
shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
for(int i=0;i<support_count_B;i++) {
supports_B[i] = transform_B->xform(supports_B[i]);
}
}
if (withMargin) {
for(int i=0;i<support_count_B;i++) {
supports_B[i]+=best_axis*margin_B;
}
}
/*
print_line("**************************");
printf("CBK: %p\n",callback->userdata);
print_line("type A: "+itos(shape_A->get_type()));
print_line("type B: "+itos(shape_B->get_type()));
print_line("xform A: "+*transform_A);
print_line("xform B: "+*transform_B);
print_line("normal: "+best_axis);
print_line("depth: "+rtos(best_depth));
print_line("index: "+itos(best_axis_index));
for(int i=0;i<support_count_A;i++) {
print_line("A-"+itos(i)+": "+supports_A[i]);
}
for(int i=0;i<support_count_B;i++) {
print_line("B-"+itos(i)+": "+supports_B[i]);
}
//*/
callback->normal=best_axis;
_generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback);
if (callback && callback->sep_axis && *callback->sep_axis!=Vector2())
*callback->sep_axis=Vector2(); //invalidate previous axis (no test)
//CollisionSolver2DSW::CallbackResult cbk=NULL;
//cbk(Vector2(),Vector2(),NULL);
}
_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;
}
_FORCE_INLINE_ void generate_contacts() {
// nothing to do, don't generate
if (best_axis == Vector3(0.0, 0.0, 0.0))
return;
if (!callback->callback) {
//just was checking intersection?
callback->collided = true;
if (callback->prev_axis)
*callback->prev_axis = best_axis;
return;
}
static const int max_supports = 16;
Vector3 supports_A[max_supports];
int support_count_A;
shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A);
for (int i = 0; i < support_count_A; i++) {
supports_A[i] = transform_A->xform(supports_A[i]);
}
if (withMargin) {
for (int i = 0; i < support_count_A; i++) {
supports_A[i] += -best_axis * margin_A;
}
}
Vector3 supports_B[max_supports];
int support_count_B;
shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B);
for (int i = 0; i < support_count_B; i++) {
supports_B[i] = transform_B->xform(supports_B[i]);
}
if (withMargin) {
for (int i = 0; i < support_count_B; i++) {
supports_B[i] += best_axis * margin_B;
}
}
/*
print_line("best depth: "+rtos(best_depth));
print_line("best axis: "+(best_axis));
for(int i=0;i<support_count_A;i++) {
print_line("A-"+itos(i)+": "+supports_A[i]);
}
for(int i=0;i<support_count_B;i++) {
print_line("B-"+itos(i)+": "+supports_B[i]);
}
*/
callback->normal = best_axis;
if (callback->prev_axis)
*callback->prev_axis = best_axis;
_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
callback->collided = true;
//CollisionSolverSW::CallbackResult cbk=NULL;
//cbk(Vector3(),Vector3(),NULL);
}