diff --git a/LivePlotter.vcxproj b/LivePlotter.vcxproj
index a0df2e6..7fe3a9d 100644
--- a/LivePlotter.vcxproj
+++ b/LivePlotter.vcxproj
@@ -140,6 +140,7 @@
+
diff --git a/LivePlotter.vcxproj.filters b/LivePlotter.vcxproj.filters
index 23a472a..4918bcd 100644
--- a/LivePlotter.vcxproj.filters
+++ b/LivePlotter.vcxproj.filters
@@ -30,5 +30,8 @@
Source Files
+
+ Source Files
+
\ No newline at end of file
diff --git a/inc/body.hpp b/inc/body.hpp
index 37b1d3e..6b3f0aa 100644
--- a/inc/body.hpp
+++ b/inc/body.hpp
@@ -4,17 +4,17 @@
#include
#include "util.hpp"
-struct body {
+struct Body {
glm::mat4 pose;
float scale;
uint ebo;
uint vao;
uint vbo;
uint shader;
- array verts;
- array faces;
+ Array verts;
+ Array faces;
glm::vec4 color;
};
-bool load_body(body* out_body, const char* obj_filepath);
-void draw_body(const body& b);
+bool load_body(Body* out_body, const char* obj_filepath);
+void draw_body(const Body& b);
diff --git a/inc/camera_poses.hpp b/inc/camera_poses.hpp
new file mode 100644
index 0000000..6d8d619
--- /dev/null
+++ b/inc/camera_poses.hpp
@@ -0,0 +1,6 @@
+#pragma once
+
+#include "util.hpp"
+#include "body.hpp"
+
+bool parse_poses(Array *bodies_out, const char* filepath);
diff --git a/inc/util.hpp b/inc/util.hpp
index fc7eb23..26b5383 100644
--- a/inc/util.hpp
+++ b/inc/util.hpp
@@ -1,6 +1,9 @@
#pragma once
#include
+#include
+#include
+#include
#include
#define min(a, b) ((a < b) ? a : b)
@@ -8,17 +11,19 @@
typedef unsigned int uint;
-template struct array {
+template struct Array {
T* data;
size_t len;
size_t cap;
inline T& operator[](int i) { return data[i]; }
};
-template void append(array& a, T el);
-template T pop(array& a);
-template void resize(array& a, size_t new_cap);
-template void clear(array& a);
-bool read_file(array* out, const char* filepath);
-array split_str(const char *s, char delimiter);
-array split_str(const char *s);
+template void append(Array& a, T el);
+template T pop(Array& a);
+template void resize(Array& a, size_t new_cap);
+template void clear(Array& a);
+bool read_file(Array* out, const char* filepath);
+Array split_str(const char* s, char delimiter);
+Array split_str(const char* s);
+
+glm::mat4 quat_to_mat4(glm::quat q);
diff --git a/src/body.cpp b/src/body.cpp
index eaa2096..2578957 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -11,8 +11,8 @@ enum class ParserState {
FACE_SKIP,
};
-bool load_body(body* out_body, const char* obj_filepath) {
- array source;
+bool load_body(Body* out_body, const char* obj_filepath) {
+ Array source;
if (!read_file(&source, obj_filepath)) {
return false;
}
@@ -27,8 +27,8 @@ bool load_body(body* out_body, const char* obj_filepath) {
}
}
- array verts = { (float*)malloc(sizeof(float) * num_verts * 3), num_verts * 3 };
- array faces = { (int*)malloc(sizeof(int) * num_faces * 3), num_faces*3 };
+ Array verts = { (float*)malloc(sizeof(float) * num_verts * 3), num_verts * 3 };
+ Array faces = { (int*)malloc(sizeof(int) * num_faces * 3), num_faces*3 };
// Get ready for the parsing loop
ParserState state = ParserState::PREFIX;
@@ -114,9 +114,9 @@ bool load_body(body* out_body, const char* obj_filepath) {
return true;
}
-void draw_body(const body& b) {
+void draw_body(const Body& b) {
use_shader(b.shader);
- set_uniform(b.shader, "global_t", b.pose);
+ set_uniform(b.shader, "global_t", b.scale*b.pose);
set_uniform(b.shader, "color", b.color);
glBindVertexArray(b.vao);
glDrawElements(GL_TRIANGLES, b.faces.len, GL_UNSIGNED_INT, 0);
diff --git a/src/camera_poses.cpp b/src/camera_poses.cpp
new file mode 100644
index 0000000..c8953d4
--- /dev/null
+++ b/src/camera_poses.cpp
@@ -0,0 +1,71 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "camera_poses.hpp"
+
+
+#define LINE_BUF_SIZE 128
+#define NUM_SPHERES_PER_AXE 25
+#define SPACE_PER_SPHERE 10.0f
+
+glm::vec4 red_color = glm::vec4(1, 0, 0, 1);
+glm::vec4 green_color = glm::vec4(0, 1, 0, 1);
+glm::vec4 blue_color = glm::vec4(0, 0, 1, 1);
+
+bool parse_poses(Array* bodies_out, const char* filepath) {
+ FILE* fp;
+ if (!fopen_s(&fp, filepath, "rb")) {
+ printf("Error parsing poses csv: %s\n", filepath);
+ return false;
+ }
+
+ char delim = ',';
+ char line[LINE_BUF_SIZE];
+
+ // read in header
+ fgets(line, LINE_BUF_SIZE, fp);
+
+ while (!feof(fp)) {
+ fgets(line, LINE_BUF_SIZE, fp);
+
+ Array words = split_str(line, delim);
+ float x = atof(words[0]);
+ float y = atof(words[1]);
+ float z = atof(words[2]);
+ float w = atof(words[3]);
+ float i = atof(words[4]);
+ float j = atof(words[5]);
+ float k = atof(words[6]);
+
+ glm::mat4 pose = glm::translate(quat_to_mat4(glm::quat(w, i, j, k)), glm::vec3(x, y, z));
+
+ // because clang refuses to cooperate with my append implementation and generate the function code...
+ *bodies_out
+ = { (Body*)malloc(sizeof(Body) * 3 * NUM_SPHERES_PER_AXE * 14), 3 * NUM_SPHERES_PER_AXE * 14 };
+
+ // Generate axis spheres
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < NUM_SPHERES_PER_AXE; j++) {
+ Body b;
+ if (!load_body(&b, "Icosphere.obj")) {
+ return false;
+ }
+
+ // How far along the axis is this ball
+ glm::vec3 trans = glm::vec3(0, 0, 0);
+ trans[i] = (float)j * SPACE_PER_SPHERE;
+
+ // Now move the translated pose via the camera's pose
+ b.pose = pose * glm::translate(b.pose, trans);
+ bodies_out->data[i*j + j] = b;
+ }
+ }
+ }
+}
diff --git a/src/main.cpp b/src/main.cpp
index f129519..a33c4ea 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -15,6 +15,7 @@
#include "util.hpp"
#include "shaders.hpp"
#include "body.hpp"
+#include "camera_poses.hpp"
static GLFWwindow* window;
static float width, height;
@@ -31,7 +32,7 @@ void process_input() {
}
static uint shader;
-void create_new_sphere(body* b, float scale=1) {
+void create_new_sphere(Body* b, float scale = 1) {
assert(load_body(b, "Icosphere.obj"));
b->shader = shader;
b->scale = scale;
@@ -40,16 +41,18 @@ void create_new_sphere(body* b, float scale=1) {
static bool stop = false;
static pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
-const float max_hp = 10; // Number of scans (without a particular barcode) for which the sphere will still be visible
-static std::vector> camera_bodies; // I would use my array here, but was getting a linking error
+const float max_hp
+ = 10; // Number of scans (without a particular barcode) for which the sphere will still be visible
+static std::vector>
+ camera_bodies; // I would use my array here, but was getting a linking error
-void *process_cin(void* args) {
+void* process_cin(void* args) {
std::string line;
while (true) {
std::getline(std::cin, line);
- array words = split_str(line.c_str());
+ Array words = split_str(line.c_str());
assert(words.len == 4);
- printf("Received: %s, %s, %s, %s\n", words[0], words[1], words[2], words[3]); // echo for debugging
+ printf("Received: %s, %s, %s, %s\n", words[0], words[1], words[2], words[3]); // echo for debugging
float x = atof(words[1]);
float y = atof(words[2]);
float z = atof(words[3]);
@@ -58,21 +61,23 @@ void *process_cin(void* args) {
pthread_mutex_lock(&lock);
for (int i = 0; i < camera_bodies.size(); i++) {
if (strcmp(words[0], std::get<0>(camera_bodies[i])) == 0 && std::get<1>(camera_bodies[i])) {
- glm::vec4 &transl = std::get<1>(camera_bodies[i])->pose[3];
- transl = 0.9f*transl + 0.1f*glm::vec4(new_loc, 1); // filter
+ glm::vec4& transl = std::get<1>(camera_bodies[i])->pose[3];
+ transl = 0.9f * transl + 0.1f * glm::vec4(new_loc, 1); // filter
int color_i = i + 1;
- std::get<1>(camera_bodies[i])->color = glm::vec4(color_i&0x4, color_i&0x2, color_i&0x1, 1);
+ std::get<1>(camera_bodies[i])->color
+ = glm::vec4(color_i & 0x4, color_i & 0x2, color_i & 0x1, 1);
std::get<2>(camera_bodies[i]) = max_hp;
found_match = true;
} else if (std::get<1>(camera_bodies[i])) {
- float &cur_hp = std::get<2>(camera_bodies[i]);
- if (cur_hp > 0) cur_hp -= 1;
- std::get<1>(camera_bodies[i])->color = glm::vec4(i&0x4, i&0x2, i&0x1, cur_hp/max_hp);
+ float& cur_hp = std::get<2>(camera_bodies[i]);
+ if (cur_hp > 0)
+ cur_hp -= 1;
+ std::get<1>(camera_bodies[i])->color = glm::vec4(i & 0x4, i & 0x2, i & 0x1, cur_hp / max_hp);
}
}
if (!found_match) {
- auto t = std::make_tuple(words[0], (body*)NULL, max_hp);
+ auto t = std::make_tuple(words[0], (Body*)NULL, max_hp);
camera_bodies.push_back(t);
}
pthread_mutex_unlock(&lock);
@@ -91,7 +96,6 @@ static glm::vec3 camera_loc = glm::vec3(0, 0, -1);
static glm::vec3 up = glm::vec3(0, 1, 0);
static glm::mat4 world_to_camera = glm::lookAt(camera_loc, focal_point, up);
-
static void cursor_position_callback(GLFWwindow* window, double xpos, double ypos) {
float dx = (xpos - prev_cursor_x);
float dy = (ypos - prev_cursor_y);
@@ -104,8 +108,8 @@ static void cursor_position_callback(GLFWwindow* window, double xpos, double ypo
prev_cursor_y = ypos;
if (mouse_pressed) {
- phi += glm::radians(dx * (360/width));// * glm::radians(360.0);
- theta += glm::radians(dy * (360/height));// * glm::radians(360.0);
+ phi += glm::radians(dx * (360 / width)); // * glm::radians(360.0);
+ theta += glm::radians(dy * (360 / height)); // * glm::radians(360.0);
double len = glm::length(camera_loc - focal_point);
camera_loc.x = focal_point.x + (len * glm::cos(theta) * glm::cos(-phi));
camera_loc.y = focal_point.y + (len * glm::sin(theta));
@@ -185,15 +189,9 @@ int main() {
glfwSwapBuffers(window); // front buffer is now back
glClear(GL_COLOR_BUFFER_BIT); // Write to back buffer again (former front buf)
- body b;
- assert(load_body(&b, "Icosphere.obj"));
- b.shader = shader;
- b.pose = glm::translate(b.pose, glm::vec3(5, 5, 5));
- b.color = glm::vec4(1, 0, 0, 1);
-
- body b2;
+ /*Body b2;
create_new_sphere(&b2);
- b2.color = glm::vec4(0, 0.5, 0, 1);
+ b2.color = glm::vec4(0, 0.5, 0, 1);*/
// glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
// set_uniform(shader, "color", glm::vec4 { sin(time), sin(time + glm::radians(45.0f)), sin(time +
@@ -208,28 +206,33 @@ int main() {
pthread_t thread_id;
pthread_create(&thread_id, NULL, process_cin, NULL);
+ Array camera_pose_axes;
+ if (!parse_poses(&camera_pose_axes, "poses.csv")) {
+ return -1;
+ }
+
while (!glfwWindowShouldClose(window)) {
process_input();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
set_uniform(shader, "camera_t", world_to_camera);
set_uniform(shader, "projection_t", projection_t);
- draw_body(b);
- draw_body(b2);
-
- pthread_mutex_lock(&lock);
-
- for (int i = 0; i < camera_bodies.size(); i++) {
- if (!std::get<1>(camera_bodies[i])) {
- body* b = (body*)malloc(sizeof(body));
- create_new_sphere(b);
- b->color = glm::vec4(i & 0x4, i& 0x2, i & 0x1, max_hp);
- std::get<1>(camera_bodies[i]) = b;
- }
- draw_body(*std::get<1>(camera_bodies[i]));
+ for (int i = 0; i < camera_pose_axes.len; i++) {
+ draw_body(camera_pose_axes[i]);
}
- pthread_mutex_unlock(&lock);
+ if (pthread_mutex_trylock(&lock)) {
+ for (int i = 0; i < camera_bodies.size(); i++) {
+ if (!std::get<1>(camera_bodies[i])) {
+ Body* b = (Body*)malloc(sizeof(Body));
+ create_new_sphere(b);
+ b->color = glm::vec4(i & 0x4, i & 0x2, i & 0x1, max_hp);
+ std::get<1>(camera_bodies[i]) = b;
+ }
+ draw_body(*std::get<1>(camera_bodies[i]));
+ }
+ pthread_mutex_unlock(&lock);
+ }
glfwSwapBuffers(window);
glfwPollEvents();
diff --git a/src/shaders.cpp b/src/shaders.cpp
index 127c456..7a3156a 100644
--- a/src/shaders.cpp
+++ b/src/shaders.cpp
@@ -68,7 +68,7 @@ void set_uniform(uint id, const char* name, uniform_variant value) {
}
bool _compile_shader(uint *out_id, const char* filepath, int shader_type) {
- array source;
+ Array source;
if (!read_file(&source, filepath))
return false;
diff --git a/src/util.cpp b/src/util.cpp
index 4030da2..8c9ab44 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -3,12 +3,14 @@
#include
#include
#include
+#include
+#include
#include "util.hpp"
-array _split_str_inner(const char* s, char delim, bool just_check_ws);
+Array _split_str_inner(const char* s, char delim, bool just_check_ws);
-template void append(array& a, T el) {
+template void append(Array& a, T el) {
if (a.len == a.cap) {
resize(a, max(8, a.cap * 2));
}
@@ -16,13 +18,13 @@ template void append(array& a, T el) {
a.len++;
}
-template T pop(array& a) {
+template T pop(Array& a) {
assert(a.len >= 1);
a.len--;
return a.data[a.len + 1];
}
-template void resize(array& a, size_t new_cap) {
+template void resize(Array& a, size_t new_cap) {
T* new_data = (T*)malloc(sizeof(T) * new_cap);
if (a.len > 0) {
memcpy(new_data, a.data, min(a.len, new_cap));
@@ -33,11 +35,11 @@ template void resize(array& a, size_t new_cap) {
a.cap = new_cap;
}
-template void clear(array& a) {
+template void clear(Array& a) {
a.len = 0;
}
-bool read_file(array* out, const char* filepath) {
+bool read_file(Array* out, const char* filepath) {
FILE* fp = NULL;
if (fopen_s(&fp, filepath, "rb") != 0) {
printf("ERROR Failed to open file %s\n", filepath);
@@ -56,16 +58,16 @@ bool read_file(array* out, const char* filepath) {
return true;
}
-array split_str(const char* s, char delimiter) { return _split_str_inner(s, delimiter, false); }
+Array split_str(const char* s, char delimiter) { return _split_str_inner(s, delimiter, false); }
-array split_str(const char* s) { return _split_str_inner(s, ' ', true); }
+Array split_str(const char* s) { return _split_str_inner(s, ' ', true); }
-array _split_str_inner(const char* s, char delim, bool just_check_ws) {
- array res = { NULL, 0, 0 };
+Array _split_str_inner(const char* s, char delim, bool just_check_ws) {
+ Array res = { NULL, 0, 0 };
char c;
int i = 0;
- array cur_word = { NULL, 0, 0 };
+ Array cur_word = { NULL, 0, 0 };
while (true) {
c = s[i++];
bool is_delim = just_check_ws ? iswspace(c) : c == delim;
@@ -89,3 +91,20 @@ array _split_str_inner(const char* s, char delim, bool just_check_ws) {
}
return res;
}
+
+
+// https://songho.ca/opengl/gl_quaternion.html
+glm::mat4 quat_to_mat4(glm::quat q) {
+ glm::vec4 col0 = glm::vec4(
+ 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, 0);
+
+ glm::vec4 col1 = glm::vec4(
+ 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, 0);
+
+ glm::vec4 col2 = glm::vec4(
+ 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y, 0);
+
+ glm::vec4 col3 = glm::vec4(0, 0, 0, 1);
+
+ return glm::mat4(col0, col1, col2, col3);
+}