nuclear@6: #include <assert.h>
nuclear@8: #include <vmath/vmath.h>
nuclear@5: #include "co_phys.h"
nuclear@6: #include "sim.h"
nuclear@8: #include "co_collider.h"
nuclear@8: 
nuclear@8: #define FIXED_MASS		1e9
nuclear@3: 
nuclear@5: static CoRigid reg_co_rigid;
nuclear@3: 
nuclear@5: static Component *cons_rigid() { return new CoRigid; }
nuclear@3: 
nuclear@5: CoRigid::CoRigid()
nuclear@3: {
nuclear@3: 	mass = 1.0;
nuclear@8: 	elast = 0.85;
nuclear@3: 	friction = 0.0;
nuclear@8: 	fixed = false;
nuclear@3: 
nuclear@3: 	name = "rigid";
nuclear@8: 	co_prs = 0;
nuclear@3: 
nuclear@3: 	register_component(name, cons_rigid);
nuclear@3: }
nuclear@3: 
nuclear@6: const char **CoRigid::update_before() const
nuclear@3: {
nuclear@6: 	static const char *before[] = { "prs", 0 };
nuclear@6: 	return before;
nuclear@3: }
nuclear@6: 
nuclear@8: void CoRigid::set_fixed(bool f)
nuclear@8: {
nuclear@8: 	fixed = f;
nuclear@8: }
nuclear@8: 
nuclear@8: bool CoRigid::get_fixed() const
nuclear@8: {
nuclear@8: 	return fixed;
nuclear@8: }
nuclear@8: 
nuclear@8: void CoRigid::set_mass(float mass)
nuclear@8: {
nuclear@8: 	this->mass = mass;
nuclear@8: }
nuclear@8: 
nuclear@8: float CoRigid::get_mass() const
nuclear@8: {
nuclear@8: 	return fixed ? FIXED_MASS : mass;
nuclear@8: }
nuclear@8: 
nuclear@6: void CoRigid::add_impulse(const Vector3 &v)
nuclear@6: {
nuclear@6: 	impulse += v;
nuclear@6: }
nuclear@6: 
nuclear@6: void CoRigid::update(float dt)
nuclear@6: {
nuclear@8: 	if(!gobj || fixed) return;
nuclear@6: 
nuclear@6: 	if(!co_prs) {
nuclear@6: 		if(!(co_prs = COCAST(CoPRS, gobj->get_component("prs")))) {
nuclear@6: 			assert(co_prs);
nuclear@6: 			return;
nuclear@6: 		}
nuclear@6: 	}
nuclear@6: 
nuclear@8: 	// collision detection
nuclear@8: 	CoCollider *co_col;
nuclear@8: 	if(world && (co_col = gobj_co_collider(gobj, COGET_FAIL))) {
nuclear@8: 		HitPoint hit;
nuclear@8: 
nuclear@8: 		// dot product subexpression is to ignore collisions when moving OUT
nuclear@8: 		// of the object after an interpenetration
nuclear@8: 		if(world->collide(co_col, &hit) && dot_product(vel, hit.normal) <= 0.0) {
nuclear@8: 			Vector3 refl = -vel.reflection(hit.normal);
nuclear@8: 			Vector3 vert = hit.normal * dot_product(refl, hit.normal);
nuclear@8: 			Vector3 tang = refl - vert;
nuclear@8: 			vel = vert * elast + tang * friction;
nuclear@8: 			// TODO add an impulse to the other object and take account of the relative masses
nuclear@8: 		}
nuclear@8: 	}
nuclear@8: 
nuclear@8: 
nuclear@6: 	float damping = world ? world->get_damping() : 0.005;
nuclear@6: 
nuclear@6: 	Vector3 newpos = co_prs->pos + vel * dt;
nuclear@6: 
nuclear@6: 	Vector3 accel = impulse;
nuclear@6: 	impulse.x = impulse.y = impulse.z = 0.0f;	// reset impulse
nuclear@6: 
nuclear@6: 	if(world) {
nuclear@6: 		accel += world->get_gravity();
nuclear@6: 	}
nuclear@6: 
nuclear@6: 	vel = (vel - vel * damping * dt) + accel * dt;
nuclear@8: 	if(vel.length() < 1e-3) {
nuclear@8: 		vel.x = vel.y = vel.z = 0.0f;
nuclear@8: 	}
nuclear@6: 	co_prs->pos = newpos;
nuclear@6: }
nuclear@8: 
nuclear@8: CoRigid *gobj_co_rigid(const GObject *obj, bool nofail)
nuclear@8: {
nuclear@8: 	CoRigid *co = COCAST(CoRigid, obj->get_component("rigid"));
nuclear@8: 	if(co) return co;
nuclear@8: 
nuclear@8: 	return nofail ? &reg_co_rigid : 0;
nuclear@8: }