Esempio n. 1
0
void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const {
	// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
	// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
	// See the comment in get_scale() for further information.
	Basis m = orthonormalized();
	real_t det = m.determinant();
	if (det < 0) {
		// Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
		m.scale(Vector3(-1, -1, -1));
	}

	m.get_axis_angle(p_axis, p_angle);
}
Esempio n. 2
0
void BodySW::integrate_forces(real_t p_step) {

	if (mode == PhysicsServer::BODY_MODE_STATIC)
		return;

	AreaSW *def_area = get_space()->get_default_area();
	// AreaSW *damp_area = def_area;

	ERR_FAIL_COND(!def_area);

	int ac = areas.size();
	bool stopped = false;
	gravity = Vector3(0, 0, 0);
	area_linear_damp = 0;
	area_angular_damp = 0;
	if (ac) {
		areas.sort();
		const AreaCMP *aa = &areas[0];
		// damp_area = aa[ac-1].area;
		for (int i = ac - 1; i >= 0 && !stopped; i--) {
			PhysicsServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
			switch (mode) {
				case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE:
				case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
					_compute_area_gravity_and_dampenings(aa[i].area);
					stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
				} break;
				case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE:
				case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
					gravity = Vector3(0, 0, 0);
					area_angular_damp = 0;
					area_linear_damp = 0;
					_compute_area_gravity_and_dampenings(aa[i].area);
					stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE;
				} break;
				default: {}
			}
		}
	}

	if (!stopped) {
		_compute_area_gravity_and_dampenings(def_area);
	}

	gravity *= gravity_scale;

	// If less than 0, override dampenings with that of the Body
	if (angular_damp >= 0)
		area_angular_damp = angular_damp;
	/*
	else
		area_angular_damp=damp_area->get_angular_damp();
	*/

	if (linear_damp >= 0)
		area_linear_damp = linear_damp;
	/*
	else
		area_linear_damp=damp_area->get_linear_damp();
	*/

	Vector3 motion;
	bool do_motion = false;

	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {

		//compute motion, angular and etc. velocities from prev transform
		linear_velocity = (new_transform.origin - get_transform().origin) / p_step;

		//compute a FAKE angular velocity, not so easy
		Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
		Vector3 axis;
		real_t angle;

		rot.get_axis_angle(axis, angle);
		axis.normalize();
		angular_velocity = axis.normalized() * (angle / p_step);

		motion = new_transform.origin - get_transform().origin;
		do_motion = true;

	} else {
		if (!omit_force_integration && !first_integration) {
			//overridden by direct state query

			Vector3 force = gravity * mass;
			force += applied_force;
			Vector3 torque = applied_torque;

			real_t damp = 1.0 - p_step * area_linear_damp;

			if (damp < 0) // reached zero in the given time
				damp = 0;

			real_t angular_damp = 1.0 - p_step * area_angular_damp;

			if (angular_damp < 0) // reached zero in the given time
				angular_damp = 0;

			linear_velocity *= damp;
			angular_velocity *= angular_damp;

			linear_velocity += _inv_mass * force * p_step;
			angular_velocity += _inv_inertia_tensor.xform(torque) * p_step;
		}

		if (continuous_cd) {
			motion = linear_velocity * p_step;
			do_motion = true;
		}
	}

	applied_force = Vector3();
	applied_torque = Vector3();
	first_integration = false;

	//motion=linear_velocity*p_step;

	biased_angular_velocity = Vector3();
	biased_linear_velocity = Vector3();

	if (do_motion) { //shapes temporarily extend for raycast
		_update_shapes_with_motion(motion);
	}

	def_area = NULL; // clear the area, so it is set in the next frame
	contact_count = 0;
}