1.1 --- a/src/co_phys.cc	Sat Feb 14 10:08:00 2015 +0200
     1.2 +++ b/src/co_phys.cc	Sun Feb 15 05:14:20 2015 +0200
     1.3 @@ -1,6 +1,10 @@
     1.4  #include <assert.h>
     1.5 +#include <vmath/vmath.h>
     1.6  #include "co_phys.h"
     1.7  #include "sim.h"
     1.8 +#include "co_collider.h"
     1.9 +
    1.10 +#define FIXED_MASS		1e9
    1.11  
    1.12  static CoRigid reg_co_rigid;
    1.13  
    1.14 @@ -9,10 +13,12 @@
    1.15  CoRigid::CoRigid()
    1.16  {
    1.17  	mass = 1.0;
    1.18 -	elast = 0.5;
    1.19 +	elast = 0.85;
    1.20  	friction = 0.0;
    1.21 +	fixed = false;
    1.22  
    1.23  	name = "rigid";
    1.24 +	co_prs = 0;
    1.25  
    1.26  	register_component(name, cons_rigid);
    1.27  }
    1.28 @@ -23,6 +29,26 @@
    1.29  	return before;
    1.30  }
    1.31  
    1.32 +void CoRigid::set_fixed(bool f)
    1.33 +{
    1.34 +	fixed = f;
    1.35 +}
    1.36 +
    1.37 +bool CoRigid::get_fixed() const
    1.38 +{
    1.39 +	return fixed;
    1.40 +}
    1.41 +
    1.42 +void CoRigid::set_mass(float mass)
    1.43 +{
    1.44 +	this->mass = mass;
    1.45 +}
    1.46 +
    1.47 +float CoRigid::get_mass() const
    1.48 +{
    1.49 +	return fixed ? FIXED_MASS : mass;
    1.50 +}
    1.51 +
    1.52  void CoRigid::add_impulse(const Vector3 &v)
    1.53  {
    1.54  	impulse += v;
    1.55 @@ -30,7 +56,7 @@
    1.56  
    1.57  void CoRigid::update(float dt)
    1.58  {
    1.59 -	if(!gobj) return;
    1.60 +	if(!gobj || fixed) return;
    1.61  
    1.62  	if(!co_prs) {
    1.63  		if(!(co_prs = COCAST(CoPRS, gobj->get_component("prs")))) {
    1.64 @@ -39,6 +65,23 @@
    1.65  		}
    1.66  	}
    1.67  
    1.68 +	// collision detection
    1.69 +	CoCollider *co_col;
    1.70 +	if(world && (co_col = gobj_co_collider(gobj, COGET_FAIL))) {
    1.71 +		HitPoint hit;
    1.72 +
    1.73 +		// dot product subexpression is to ignore collisions when moving OUT
    1.74 +		// of the object after an interpenetration
    1.75 +		if(world->collide(co_col, &hit) && dot_product(vel, hit.normal) <= 0.0) {
    1.76 +			Vector3 refl = -vel.reflection(hit.normal);
    1.77 +			Vector3 vert = hit.normal * dot_product(refl, hit.normal);
    1.78 +			Vector3 tang = refl - vert;
    1.79 +			vel = vert * elast + tang * friction;
    1.80 +			// TODO add an impulse to the other object and take account of the relative masses
    1.81 +		}
    1.82 +	}
    1.83 +
    1.84 +
    1.85  	float damping = world ? world->get_damping() : 0.005;
    1.86  
    1.87  	Vector3 newpos = co_prs->pos + vel * dt;
    1.88 @@ -51,8 +94,16 @@
    1.89  	}
    1.90  
    1.91  	vel = (vel - vel * damping * dt) + accel * dt;
    1.92 -
    1.93 -	// TODO collisions
    1.94 -
    1.95 +	if(vel.length() < 1e-3) {
    1.96 +		vel.x = vel.y = vel.z = 0.0f;
    1.97 +	}
    1.98  	co_prs->pos = newpos;
    1.99  }
   1.100 +
   1.101 +CoRigid *gobj_co_rigid(const GObject *obj, bool nofail)
   1.102 +{
   1.103 +	CoRigid *co = COCAST(CoRigid, obj->get_component("rigid"));
   1.104 +	if(co) return co;
   1.105 +
   1.106 +	return nofail ? &reg_co_rigid : 0;
   1.107 +}