nuclear@2: #include <float.h>
nuclear@0: #include "simworld.h"
nuclear@0: 
nuclear@0: SimWorld::SimWorld()
nuclear@0: {
nuclear@0: 	grav = Vector3(0, -9.81, 0);
nuclear@0: 	damping = 0.99;
nuclear@0: }
nuclear@0: 
nuclear@3: void SimWorld::add_object(Object *o)
nuclear@3: {
nuclear@3: 	objects.push_back(o);
nuclear@3: }
nuclear@3: 
nuclear@3: int SimWorld::get_object_count() const
nuclear@3: {
nuclear@3: 	return (int)objects.size();
nuclear@3: }
nuclear@3: 
nuclear@3: Object *SimWorld::get_object(int idx) const
nuclear@3: {
nuclear@3: 	return objects[idx];
nuclear@3: }
nuclear@3: 
nuclear@0: void SimWorld::add_particle(Particle *p)
nuclear@0: {
nuclear@0: 	part.push_back(p);
nuclear@0: }
nuclear@0: 
nuclear@0: bool SimWorld::collision(const Ray &ray, float rad, Collision *col) const
nuclear@0: {
nuclear@2: 	Collision nearest;
nuclear@2: 	nearest.dist = FLT_MAX;
nuclear@0: 
nuclear@2: 	for(auto obj : objects) {
nuclear@2: 		Collision tmpcol;
nuclear@2: 		if(obj->collision(ray, rad, &tmpcol) && tmpcol.dist < nearest.dist) {
nuclear@2: 			nearest = tmpcol;
nuclear@2: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@2: 	*col = nearest;
nuclear@2: 	return true;
nuclear@0: }
nuclear@0: 
nuclear@0: void SimWorld::step(float dt)
nuclear@0: {
nuclear@0: 	for(size_t i=0; i<part.size(); i++) {
nuclear@0: 		// add gravity
nuclear@0: 		part[i]->add_force(grav * part[i]->get_mass());
nuclear@0: 
nuclear@0: 		part[i]->step(this, dt);
nuclear@0: 
nuclear@0: 		// handle collisions with other particles
nuclear@0: 		for(size_t j=0; j<part.size(); j++) {
nuclear@0: 			Collision col;
nuclear@0: 			if(i != j && part[i]->collision(part[j], &col)) {
nuclear@0: 				Vector3 rel_vel = part[j]->get_velocity() - part[i]->get_velocity();
nuclear@0: 
nuclear@0: 				float kn = 1.0 / part[i]->get_mass() + 1.0 / part[j]->get_mass();
nuclear@0: 				float imp = dot_product(rel_vel, col.normal) * (col.elast + 1) / kn;
nuclear@0: 
nuclear@0: 				if(imp < 0.0) imp = 0.0;
nuclear@0: 
nuclear@0: 				Vector3 v = part[i]->get_position() - part[j]->get_position();
nuclear@0: 				float dist_sq = v.length_sq();
nuclear@0: 				float pen_depth_sq = part[i]->get_radius() + part[j]->get_radius() - dist_sq;
nuclear@0: 				if(pen_depth_sq < 0.0) pen_depth_sq = 0.0;
nuclear@0: 
nuclear@0: 				part[i]->add_force(col.normal * imp * (1.0 / dt * pen_depth_sq + 1));
nuclear@0: 			}
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: }
nuclear@0: 
nuclear@0: void SimWorld::draw_particles() const
nuclear@0: {
nuclear@0: 	for(size_t i=0; i<part.size(); i++) {
nuclear@0: 		part[i]->draw();
nuclear@0: 	}
nuclear@0: }