extends RigidBody
class_name BuoyantBody

export(NodePath) var water = NodePath("/root/Main/Water")
onready var _water = get_node(water)

var volume_half = 0.0
var volume = 0.0
var bounding_box = []
var center_of_mass = Vector3.ZERO
var volume_up = Vector3.UP

onready var inv_gravity = -ProjectSettings.get_setting("physics/3d/default_gravity_vector") * ProjectSettings.get_setting("physics/3d/default_gravity")
const POINT_VOLUME_WEIGHT = 0.125

func _ready():
    for c in get_children():
        if c is CollisionShape:
            if c.shape is SphereShape:
                _init_buoyancy(c.shape.radius,c.shape.radius,c.shape.radius,c.transform)
            elif c.shape is CapsuleShape:
                _init_buoyancy(c.shape.radius,c.shape.radius,c.shape.height/2.0,c.transform)
            elif c.shape is BoxShape:
                _init_buoyancy(c.shape.extents.x,c.shape.extents.y,c.shape.extents.z,c.transform)
            else:
                assert(false,"this collision shape isn't supported for buoyancy")
            break

func _init_buoyancy(x_rad,y_rad,z_rad,xform):
    self.volume_half = x_rad * y_rad * z_rad * 4.0
    self.volume = volume_half * 2.0
    var aabb = [
        Vector3(-x_rad,-y_rad,-z_rad),
        Vector3(-x_rad,-y_rad, z_rad),
        Vector3(-x_rad, y_rad,-z_rad),
        Vector3(-x_rad, y_rad, z_rad),
        Vector3( x_rad,-y_rad,-z_rad),
        Vector3( x_rad,-y_rad, z_rad),
        Vector3( x_rad, y_rad,-z_rad),
        Vector3( x_rad, y_rad, z_rad)
    ]
    for p in aabb:
        self.bounding_box.push_back(xform * p)
    self.center_of_mass = xform * center_of_mass
    self.volume_up = (xform.basis * Vector3.UP).normalized()

func _physics_process(_delta):
    #check if points are submerged
    #also weight them based on how deep they are to help get a weighted average center point later
    var submerged = []
    var sub_weight = []
    var sub_total = 0.0
    for p in bounding_box:
        var g_p = self.global_transform * p
        var wave_height = _water.height(g_p)
        var diff = g_p.y - wave_height
        if diff < 0.0:
            submerged.push_back(p)
            sub_weight.push_back(diff)
            sub_total += diff
    #if no points are submerged, the whole thing is likely above water
    #therefore, no buoyant force is applied
    if submerged.size() == 0:
        return
    #if all points are submerged, the whole thing is likely below water
    #so the buoyant force would be the weight of the water displaced by the entire volume
    elif submerged.size() == 8:
        add_central_force(inv_gravity * volume)
    #if only some points are submerged, we need to estimate the amount of the volume displacing water
    #the weight of that water is the buoyant force
    else:
        #we want to apply the buoyant force to the center of mass of the submerged part of the volume
        #we can estimate it with a weighted average of the submerged points
        var force_pnt = Vector3.ZERO
        for p in range(submerged.size()):
            force_pnt += submerged[p] * sub_weight[p]
        force_pnt /= sub_total
        #apply_force uses global rotation but local origin...
        force_pnt = self.global_transform.basis * force_pnt
        #to estimate the submerged part of the volume,
        #we can see how deep into the water the bottom of an axis-aligned bounding box is
        #and do an easing and lerp over the volume
        var lowest = (self.global_transform * bounding_box[0]).y
        for p in bounding_box:
            var p_h = (self.global_transform * p).y
            if p_h < lowest:
                lowest = p_h
        var center = self.global_transform * center_of_mass
        var depth = _water.height(center) - lowest
        #the water isn't a flat plane
        #it's possible that some points are submerged,
        #but the lowest point of the axis-aligned bounding box isn't actually under water
        #in that case, for simplicity,
        #just fall back to the old method of applying a small, constant force
        #proportional to the number of submerged points
        if depth <= 0.0:
            add_force(inv_gravity * volume * POINT_VOLUME_WEIGHT * submerged.size(), force_pnt)
        #lerp the volume against the depth of the axis-aligned bounding box
        #but that lerp is only linear when the actual bounding box is axis-aligned
        #for simplicity, we can lerp an easing curve based on how aligned the bounding box is
        #on one end, it's linear and on the other it uses a basic exponential easing
        #we want to do an inverse exponential ease for the upper half of the volume if it is also submerged
        else:
            var up = self.global_transform.basis * volume_up
            var aligned = up.dot(Vector3.UP)
            aligned = abs(aligned)
            aligned -= 0.5
            aligned = abs(aligned)
            aligned *= 2.0
            var easing_curve = lerp(4.8,1.0,aligned)
            var breadth = (center.y - lowest)
            depth = clamp(depth,0.0,breadth*2.0)
            depth /= breadth
            var v = clamp(depth,0.0,1.0)
            v = ease(v,easing_curve)
            var v2 = clamp(depth-1.0,0.0,1.0)
            v2 = 1.0 - ease(1.0 - v2,easing_curve)
            v += v2
            v = lerp(0.0,volume_half,v)
            add_force(inv_gravity * v, force_pnt)