Fix math for sampling water height for buoyancy

1 files changed, 9 insertions, 12 deletions

diff --git a/water/Water.gd b/water/Water.gd
index 32f89ca..d8027f8 100644
--- a/water/Water.gd
+++ b/water/Water.gd
@@ -98,18 +98,15 @@ func gertsner_wave(pos, idx):
     return new_pos
 
 func sample_wave(pos):
-    var new_pos = Vector3(pos.x,0,pos.z)
+    var offset = Vector3.ZERO
     for idx in waves.size():
-        new_pos += gertsner_wave(pos, idx)
-    return new_pos
+        offset += gertsner_wave(pos, idx)
+    return offset
 
 func height(pos):
-    var old_pos = pos
-    var y = old_pos.y
-    for _i in range(2):
-        var new_pos = sample_wave(old_pos)
-        y = new_pos.y
-        new_pos = old_pos - (new_pos - old_pos)
-        old_pos = new_pos
-    y += self.water_plane.global_transform.origin.y
-    return y
+    var test_pos = pos
+    var offset = Vector3.ZERO
+    for _i in range(3):
+        offset = sample_wave(test_pos)
+        test_pos += (pos - (test_pos + offset))
+    return offset.y + self.water_plane.global_transform.origin.y