1 #include <float.h>

2 #include "simworld.h"

3

4 SimWorld::SimWorld()

5 {

6 grav = Vector3(0, -9.81, 0);

7 damping = 0.99;

8 }

9

10 void SimWorld::add_particle(Particle *p)

11 {

12 part.push_back(p);

13 }

14

15 bool SimWorld::collision(const Ray &ray, float rad, Collision *col) const

16 {

17 Collision nearest;

18 nearest.dist = FLT_MAX;

19

20 for(auto obj : objects) {

21 Collision tmpcol;

22 if(obj->collision(ray, rad, &tmpcol) && tmpcol.dist < nearest.dist) {

23 nearest = tmpcol;

24 }

25 }

26

27 *col = nearest;

28 return true;

29 }

30

31 void SimWorld::step(float dt)

32 {

33 for(size_t i=0; i<part.size(); i++) {

34 // add gravity

35 part[i]->add_force(grav * part[i]->get_mass());

36

37 part[i]->step(this, dt);

38

39 // handle collisions with other particles

40 for(size_t j=0; j<part.size(); j++) {

41 Collision col;

42 if(i != j && part[i]->collision(part[j], &col)) {

43 Vector3 rel_vel = part[j]->get_velocity() - part[i]->get_velocity();

44

45 float kn = 1.0 / part[i]->get_mass() + 1.0 / part[j]->get_mass();

46 float imp = dot_product(rel_vel, col.normal) * (col.elast + 1) / kn;

47

48 if(imp < 0.0) imp = 0.0;

49

50 Vector3 v = part[i]->get_position() - part[j]->get_position();

51 float dist_sq = v.length_sq();

52 float pen_depth_sq = part[i]->get_radius() + part[j]->get_radius() - dist_sq;

53 if(pen_depth_sq < 0.0) pen_depth_sq = 0.0;

54

55 part[i]->add_force(col.normal * imp * (1.0 / dt * pen_depth_sq + 1));

56 }

57 }

58 }

59 }

60

61 void SimWorld::draw_particles() const

62 {

63 for(size_t i=0; i<part.size(); i++) {

64 part[i]->draw();

65 }

66 }