_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;
	}
Beispiel #2
0
	_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);

	}
Beispiel #3
0
	_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);
	}