Added more math functions and sanity checks

No +/-Inf or NaN in my vectors please :)

6 files changed, 39 insertions, 2 deletions

diff --git a/Djup.Native/LibDjup.cs b/Djup.Native/LibDjup.cs
index 0ecf7a8..0b4a3db 100644
--- a/Djup.Native/LibDjup.cs
+++ b/Djup.Native/LibDjup.cs
@@ -16,6 +16,15 @@ public static partial class LibDjup
     [LibraryImport(LibraryName, EntryPoint = Prefix + nameof(get_vec2))]
     public static partial Vec2 get_vec2();
 
+    [LibraryImport(LibraryName, EntryPoint = Prefix + nameof(vec2_mul))]
+    public static partial Vec2 vec2_mul(Vec2 v, float scalar);
+
+    [LibraryImport(LibraryName, EntryPoint = Prefix + nameof(vec2_normal))]
+    public static partial Vec2 vec2_normal(Vec2 v);
+
+    [LibraryImport(LibraryName, EntryPoint = Prefix + nameof(vec2_length))]
+    public static partial float vec2_length(Vec2 v);
+
     [LibraryImport(LibraryName, EntryPoint = Prefix + nameof(log_set_output))]
     public static partial Vec2 log_set_output([MarshalAs(UnmanagedType.FunctionPtr)] LoggerDelegate logger);
 
diff --git a/Djup.Native/Types.cs b/Djup.Native/Types.cs
index 572277f..478208f 100644
--- a/Djup.Native/Types.cs
+++ b/Djup.Native/Types.cs
@@ -1,3 +1,4 @@
+using System.Diagnostics;
 using System.Runtime.InteropServices;
 
 namespace Djup.Native;
@@ -10,6 +11,9 @@ public struct Vec2
 
     public Vec2(float x, float y)
     {
+        Debug.Assert(float.IsFinite(x));
+        Debug.Assert(float.IsFinite(y));
+
         X = x;
         Y = y;
     }
diff --git a/client/Root.cs b/client/Root.cs
index b565083..dfbf0da 100644
--- a/client/Root.cs
+++ b/client/Root.cs
@@ -37,7 +37,8 @@ public partial class Root : Node2D
 			balls[i] = id;
 			Entity *ent;
 			var pos = new Vec2(5f * rng.Next(10), 5f * rng.Next(20));
-			var vel = new Vec2(50f + 5f * rng.Next(10), 50f);
+			var vel = LibDjup.vec2_mul(LibDjup.vec2_normal(new Vec2(50f + 5f * rng.Next(10), 50f)), 25.0f);
+			GD.Print(vel, " --> ", LibDjup.vec2_length(vel));
 			ent = LibDjup.world_find_entity(native_world, balls[i]);
 			ent->ball.Position = pos;
 			ent->ball.Velocity = vel;
diff --git a/lib/build.sh b/lib/build.sh
index 725c237..f305939 100755
--- a/lib/build.sh
+++ b/lib/build.sh
@@ -7,7 +7,7 @@ fail() {
 
 CC=cc
 CFLAGS="-std=c99 -Wall -Wextra -Wpedantic -D_POSIX_C_SOURCE=200809L"
-LDFLAGS="-fPIC"
+LDFLAGS="-fPIC -lm"
 SOURCES="test.c vec2.c world.c log.c"
 output="build"
 
diff --git a/lib/types.h b/lib/types.h
index 0619c8a..8618df9 100644
--- a/lib/types.h
+++ b/lib/types.h
@@ -8,3 +8,5 @@ vec2 dp_vec2_add(vec2, vec2);
 vec2 dp_vec2_sub(vec2, vec2);
 vec2 dp_vec2_mul(vec2, float scalar);
 vec2 dp_vec2_dot(vec2, vec2);
+float dp_vec2_length(vec2);
+vec2 dp_vec2_normal(vec2);
diff --git a/lib/vec2.c b/lib/vec2.c
index b417e9c..d795d50 100644
--- a/lib/vec2.c
+++ b/lib/vec2.c
@@ -1,9 +1,16 @@
+#include <assert.h>
+#include <math.h>
+
 #include "types.h"
 
 vec2
 dp_vec2_new(float x, float y)
 {
 	vec2 new;
+	
+	assert(isfinite(x));
+	assert(isfinite(y));
+
 	new.x = x;
 	new.y = y;
 	return new;
@@ -44,3 +51,17 @@ dp_vec2_dot(vec2 a, vec2 b)
 	dot.y = a.y * b.y;
 	return dot;
 }
+
+float
+dp_vec2_length(vec2 v)
+{
+	return sqrtf(powf(v.x, 2.f) + powf(v.y, 2.f));
+}
+
+vec2
+dp_vec2_normal(vec2 v)
+{
+	float a;
+	a = 1.f / dp_vec2_length(v);
+	return dp_vec2_mul(v, a);
+}