#include "force_generator.hpp"

namespace physics {

  void gravity::update_force(rigid_body * body, float duration)
  {
    body->force_accum += gravity * body->get_mass();
  }

  void spring::update_force(rigid_body * body, float duration)
  {
    vec3 pa = body->transform_matrix * connection_point;
    vec3 pb = other->transform_matrix * other_connection_point;

    vec3 force = pa - pb;

    float mag = magnitude(force);
    float intensity = abs(mag - rest_length) * spring_constant;

    body->add_force_at_point(normalize(force) * -intensity, pa);
  }

  void buoyancy::update_force(rigid_body * body, float duration)
  {
    vec3 center_of_buoyancy_world = body->transform_matrix * center_of_buoyancy;
    float depth = center_of_buoyancy_world.y;

    if (depth >= water_height + max_depth)
      return; // not inside the water

    vec3 force(0, 0, 0);

    if (depth <= water_height - max_depth) {
      // fully submerged
      force.y = liquid_density * volume;
    } else {
      // partially submerged
      force.y = liquid_density * volume * (depth - max_depth - water_height) / 2.0f * max_depth;
    }
    body->add_force_at_point(force, center_of_buoyancy);
  }
}
