1 files changed, 120 insertions, 0 deletions
diff --git a/vector.ino b/vector.ino
new file mode 100644
index 0000000..d56cf85
--- /dev/null
+++ b/vector.ino
@@ -0,0 +1,120 @@
+#include "vector.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <assert.h>
+
+static inline vector_t *vector_exists(vector_t *v)
+{
+	assert(v);
+
+	return v;
+}
+
+// Overwrites stuff in p. If p is null a new vector is created
+vector_t *vector_set(vector_t *p, COORD_T x, COORD_T y, COORD_T z)
+{
+	p = vector_exists(p);
+
+	p->x = x;
+	p->y = y;
+	p->z = z;
+
+	return p;
+}
+
+// Can also be used to create a vector by leaving v NULL
+vector_t *vector_copy(vector_t *dest, vector_t *src)
+{
+	dest = vector_exists(dest);
+
+	// If v exist copy it over
+	if (src) {
+		memcpy(dest, src, sizeof(vector_t));
+	}
+
+	return dest;
+}
+
+COORD_T vector_len(vector_t *v)
+{
+	return sqrt( v->x * v->x + v->y * v->y + v->z * v->z );
+}
+
+vector_t *vector_add(vector_t *dest, vector_t *a, vector_t *b)
+{
+	dest = vector_exists(dest);
+
+	dest->x = a->x + b->x;
+	dest->y = a->y + b->y;
+	dest->z = a->z + b->z;
+
+	return dest;
+}
+
+vector_t *vector_sub(vector_t *dest, vector_t *a, vector_t *b)
+{
+	dest = vector_exists(dest);
+
+	dest->x = a->x - b->x;
+	dest->y = a->y - b->y;
+	dest->z = a->z - b->z;
+
+	return dest;
+}
+
+vector_t *vector_mult(vector_t *dest, vector_t *a, vector_t *b)
+{
+	dest = vector_exists(dest);
+
+	dest->x = a->x * b->x;
+	dest->y = a->y * b->y;
+	dest->z = a->z * b->z;
+
+	return dest;
+}
+
+vector_t *vector_scale(vector_t *dest, vector_t *a, COORD_T b)
+{
+	dest = vector_exists(dest);
+
+	dest->x = a->x * b;
+	dest->y = a->y * b;
+	dest->z = a->z * b;
+
+	return dest;
+}
+
+vector_t *vector_scale_inv(vector_t *dest, vector_t *a, COORD_T b)
+{
+	dest = vector_exists(dest);
+
+	dest->x = a->x / b;
+	dest->y = a->y / b;
+	dest->z = a->z / b;
+
+	return dest;
+}
+
+COORD_T vector_dot(vector_t *a, vector_t *b)
+{
+	return a->x * b->x + a->y * b->y + a->z * b->z;
+}
+
+vector_t *vector_cross(vector_t *dest, vector_t *a, vector_t *b)
+{
+	dest = vector_exists(dest);
+
+	dest->x = a->y * b->z - a->z * b->y;
+	dest->y = a->z * b->x - a->x * b->z;
+	dest->z = a->x * b->y - a->y * b->x;
+
+	return dest;
+}
+
+void vector_print(vector_t *v)
+{
+	Serial.println("Not implemented");
+}