Initial

CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+include(FetchContent)
+# Set the project name. This is not the executable program's name!
+project(graphical_template)
+if(NOT EMSCRIPTEN)
+    set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined,address")
+endif()
+FetchContent_Declare(
+    raylib
+    URL https://github.com/manuel5975p/raylib/archive/refs/tags/5.1.1.tar.gz
+    DOWNLOAD_EXTRACT_TIMESTAMP True #This option is not required but suppresses a warning
+)
+FetchContent_MakeAvailable(raylib)
+add_executable(index "hexagon.cpp")
+
+# Link raylib to main
+
+target_include_directories(index PUBLIC "${raylib_SOURCE_DIR}/src")
+FetchContent_Declare(
+	eigen3
+    URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz
+)
+
+FetchContent_MakeAvailable(eigen3)
+file(
+    DOWNLOAD
+    https://raw.githubusercontent.com/raysan5/raygui/4.0/src/raygui.h
+    "${CMAKE_BINARY_DIR}/downloaded_headers/raygui_dl.h"
+)
+include_directories(${CMAKE_BINARY_DIR}/downloaded_headers/)
+target_link_libraries(index PUBLIC raylib Eigen3::Eigen)
+target_compile_features(index PUBLIC cxx_std_20)
+if (EMSCRIPTEN)
+    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -lidbfs.js -s USE_GLFW=3 --shell-file ${CMAKE_CURRENT_LIST_DIR}/web/minshell.html -s GL_ENABLE_GET_PROC_ADDRESS=1")
+    set(CMAKE_EXECUTABLE_SUFFIX ".html") # This line is used to set your executable to build with the emscripten html template so that you can directly open it.
+endif ()

hexagon.cpp

@@ -0,0 +1,313 @@
+#define Matrix rlMatrix
+#define Vector3 rlVector3
+#define Quaternion rlQuaternion
+#include <raylib.h>
+#include <rlgl.h>
+#include <external/glad.h>
+#define RAYGUI_IMPLEMENTATION
+#include <raygui_dl.h>
+#undef Quaternion
+#undef Vector3
+#undef Matrix
+#include "jet.hpp"
+#include <array>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <iostream>
+#include <cmath>
+#ifdef __EMSCRIPTEN__
+#include <emscripten.h>
+#endif
+using namespace Eigen;
+template<typename T>
+using Vector6 = Eigen::Vector<T, 6>;
+template<typename T>
+using Matrix6 = Eigen::Matrix<T, 6, 6>;
+
+template<typename scalar>
+    requires(scalar::n_vars >= 6)
+struct transform{
+    Vector3<scalar> translation;
+    Vector3<scalar> euler_rotation_angles;
+    transform() : translation(0,0,0), euler_rotation_angles(0,0,0){
+        for(size_t i = 0;i < 3;i++){
+            translation[i].deriv(i) = 1;
+            euler_rotation_angles[i].deriv(i + 3) = 1;
+        }
+    }
+    Matrix3<scalar> matrix()const noexcept{
+        return quat().toRotationMatrix();
+    }
+    Quaternion<scalar> quat()const noexcept{
+        Eigen::AngleAxis<scalar> rollAngle(euler_rotation_angles.x(), Eigen::Vector3<scalar>::UnitX());
+        Eigen::AngleAxis<scalar> yawAngle(euler_rotation_angles.y(), Eigen::Vector3<scalar>::UnitY());
+        Eigen::AngleAxis<scalar> pitchAngle(euler_rotation_angles.z(), Eigen::Vector3<scalar>::UnitZ());
+        return rollAngle * yawAngle * pitchAngle;
+    }
+    template<int N>
+    Matrix<scalar, 3, N> apply(const Matrix<scalar, 3, N>& input){
+        Matrix<scalar, 3, N> trfed = matrix() * input;
+        trfed.colwise() += translation;
+        return trfed;
+    }
+};
+
+template<typename T>
+Matrix<T, 3, 6> generate_hexagon(T scale, Vector3<T> translation = Vector3<T>(0, 0, 0)){
+    T scale3 = scale * std::sqrt(T(3));
+    T scale32 = scale3 / T(2);
+    translation.x() -= scale * 0.5;
+    translation.y() -= scale32;
+    Matrix<T, 3, 6> vertices;
+    vertices.col(0) = translation + Vector3<T>(0, 0, 0);
+    vertices.col(1) = translation + Vector3<T>(scale, 0, 0);
+    vertices.col(2) = translation + Vector3<T>(scale * 1.5, scale32, 0);
+    vertices.col(3) = translation + Vector3<T>(scale, scale3, 0);
+    vertices.col(4) = translation + Vector3<T>(0, scale3, 0);
+    vertices.col(5) = translation + Vector3<T>(-scale * 0.5, scale32, 0);
+    return vertices;
+}
+template<typename T>
+Matrix<T, 3, 6> generate_scronched_hexagon(T scale, Vector3<T> translation = Vector3<T>(0, 0, 0)){
+    T scale3 = scale * std::sqrt(T(3));
+    T scale32 = scale3 / T(2);
+    translation.x() -= scale * 0.5;
+    translation.y() -= scale32;
+    Matrix<T, 3, 6> vertices;
+    vertices.col(0) = translation + Vector3<T>(0, 0, 0);
+    vertices.col(1) = translation + Vector3<T>(0, 0, 0);
+    vertices.col(2) = translation + Vector3<T>(scale * 1.5, scale32, 0);
+    vertices.col(3) = translation + Vector3<T>(scale * 1.5, scale32, 0);
+    vertices.col(4) = translation + Vector3<T>(0, scale3, 0);
+    vertices.col(5) = translation + Vector3<T>(0, scale3, 0);
+    return vertices;
+}
+int GuiSlider(Rectangle bounds, const char *textLeft, const char *textRight, double *value, float minValue, float maxValue){
+    float v = *value;
+    int res = GuiSlider(bounds, textLeft, textRight, &v, minValue, maxValue);
+    *value = v;
+    return res;
+}
+
+template<typename scalar>
+using jet_type = jet<scalar, 12>;
+template<typename scalar>
+using hexagon = Matrix<jet_type<scalar>, 3, 6>;
+
+constexpr double distance = 1.5;
+template<typename scalar>
+std::pair<Vector6<scalar>, Matrix6<scalar>> evaluate_with_jacobian(const transform<jet_type<scalar>>& trf, const hexagon<scalar>& points, const hexagon<scalar>& anchors){
+    hexagon<scalar> trfed = trf.matrix() * points;
+    trfed.colwise() += trf.translation;
+    Vector6<jet_type<scalar>> distances;
+    for(size_t i = 0;i < 6;i++){
+        distances(i) = (trfed.col(i) - anchors.col(i)).norm();
+    }
+    std::pair<Vector6<scalar>, Matrix6<scalar>> ret;
+    for(size_t i = 0;i < 6;i++){
+        ret.first(i) = distances(i).value - distance;
+        ret.second.array().row(i) = distances(i).deriv.template head<6>();
+    }
+    return ret;
+}
+template<typename scalar>
+std::pair<Vector6<scalar>, Matrix6<scalar>> evaluate_with_jacobian_wrt_actuators(const transform<jet_type<scalar>>& trf, const hexagon<scalar>& points, const hexagon<scalar>& anchors){
+    hexagon<scalar> trfed = trf.matrix() * points;
+    trfed.colwise() += trf.translation;
+    Vector6<jet_type<scalar>> distances;
+    for(size_t i = 0;i < 6;i++){
+        distances(i) = (trfed.col(i) - anchors.col(i)).norm();
+    }
+    std::pair<Vector6<scalar>, Matrix6<scalar>> ret;
+    for(size_t i = 0;i < 6;i++){
+        ret.first(i) = distances(i).value - distance;
+        ret.second.array().row(i) = distances(i).deriv.template tail<6>();
+    }
+    return ret;
+}
+template<typename T, int N>
+std::array<rlVector3, N> convert_to_raylib(const Matrix<T, 3, N>& mat){
+    std::array<rlVector3, N> ret;
+    for(int i = 0;i < N;i++){
+        ret[i] = rlVector3(mat.col(i)[0], mat.col(i)[1], mat.col(i)[2]);
+    }
+    return ret;
+}
+rlVector3 erl(const auto& v){
+    rlVector3 ret(v(0), v(1), v(2));
+    return ret;
+}
+RenderTexture vp;
+float camangle = 0.0f;
+float campitch = 1.0f;
+bool camdragging = false;
+Vector6<float> actuator_angles = Vector6<float>::Zero();
+transform<jet_type<double>> current_transform; // Identity
+
+void gameLoop(){
+    using scalar = double;
+    rlVector3 targ{0.5f,std::sqrt(3.0f) / 2.0f,0};
+    campitch = std::clamp(campitch, -1.2f, 1.2f);
+    Camera3D cam{.position = rlVector3{std::sin(camangle) * 4.0f + targ.x,std::cos(camangle) * 4.0f + targ.y, std::tan(campitch)}, .target = targ, .up = {0,0,1},.fovy =  60.0f, .projection =  CAMERA_PERSPECTIVE};
+    BeginDrawing();
+    if(!IsMouseButtonDown(MOUSE_LEFT_BUTTON)){
+        camdragging = false;
+    }
+    if(IsMouseButtonPressed(MOUSE_LEFT_BUTTON)){
+        if(GetMouseX() <= 1000){
+            //std::cout << "oof\n";
+            camdragging = true;
+        }
+    }else if(IsMouseButtonDown(MOUSE_BUTTON_LEFT) && camdragging){
+        camangle += GetMouseDelta().x * 0.01f;
+        campitch += GetMouseDelta().y * 0.01f;
+    }
+    
+    ClearBackground(Color(30,30,10,255));
+    BeginTextureMode(vp);
+    ClearBackground(Color(30,30,10,255));
+    BeginMode3D(cam);
+    auto unit_hexagon = generate_scronched_hexagon(jet_type<scalar>(1.0));
+    auto anchors = generate_hexagon(jet_type<scalar>(1.6), Vector3<jet_type<scalar>>(0, 0, -1.0));
+    std::array<std::pair<Vector3<double>, Vector3<double>>, 6> rotary_planes;
+    double rad = 0.4;
+    for(int i = 0;i < 6;i++){
+        Vector3<double> diag = anchors.col((i + 3) % 6).cast<double>() - anchors.col(i).cast<double>();
+        rotary_planes[i].first = Vector3<double>(-diag.y(), diag.x(), 0).normalized();
+        rotary_planes[i].second = Vector3<double>(0,0,1);
+        
+        for(int j = 0;j < 128;j++){
+            double angle = 2.0 * M_PI * double(j) / 127;
+            double anglen = 2.0 * M_PI * double(j + 1) / 127;
+            Vector3<double> from = anchors.col(i).cast<double>() + rotary_planes[i].first * std::cos(angle) * rad + rotary_planes[i].second * std::sin(angle)  * rad;
+            Vector3<double> to = anchors.col(i).cast<double>() + rotary_planes[i].first * std::cos(anglen)  * rad + rotary_planes[i].second * std::sin(anglen) * rad;
+            rlVector3 rlf = erl(from);
+            rlVector3 rlt = erl(to);
+            DrawLine3D(rlf, rlt, ORANGE);
+        }
+    }
+    for(int i = 0;i < 6;i++){
+        jet_type<double> actuator_angle_jet(actuator_angles[i]);
+        actuator_angle_jet.deriv(i + 6) = 1;
+        anchors.col(i) += (jet_type<double>(rad) * cos(actuator_angle_jet) * rotary_planes[i].first .cast<jet_type<double>>())
+                       +  (jet_type<double>(rad) * sin(actuator_angle_jet) * rotary_planes[i].second.cast<jet_type<double>>());
+    }
+    //anchors.col(0)(2) += 0.1 * std::sin(5.0 * GetTime());
+    auto trf_backup = current_transform;
+    current_transform = transform<jet_type<double>>();
+    for(int i = 0;i < 20;i++){
+        auto[v, J] = evaluate_with_jacobian<double>(current_transform, unit_hexagon, anchors);
+        FullPivHouseholderQR<Matrix6<double>> qr(J);
+        
+        Vector6<double> subtract = qr.solve(v);
+        current_transform.translation -= subtract.head<3>().cast<jet_type<double>>();
+        current_transform.euler_rotation_angles -= subtract.segment(3, 3).cast<jet_type<double>>();
+        //std::cout << v << "\n";
+        //std::cout << "Rank: " << J.fullPivHouseholderQr().rank() << "\n";
+        //std::cout << "Rank: " << J.fullPivHouseholderQr().solve(v) << "\n";
+        auto[vi, Ji] = evaluate_with_jacobian_wrt_actuators<double>(current_transform, unit_hexagon, anchors);
+        if(vi.norm() > 0.1){
+            current_transform = trf_backup;
+        }
+    }
+    
+    Matrix<double, 3, 6> hexagon_vertices = current_transform.apply(unit_hexagon).cast<double>();
+    std::array<rlVector3, 6> hvrl = convert_to_raylib(hexagon_vertices);
+    std::array<rlVector3, 6> arl = convert_to_raylib(anchors);
+    for(int i = 0;i < 6;i++){
+        DrawLine3D(hvrl[i], arl[i], WHITE);
+    }
+    DrawTriangle3D(hvrl[0], hvrl[2], hvrl[4], Color{255, 255, 0, 120});
+    EndMode3D();
+    EndTextureMode();
+    SetTextureFilter(vp.texture, TEXTURE_FILTER_BILINEAR);
+    GenTextureMipmaps(&vp.texture);
+    DrawTexturePro(vp.texture, Rectangle(0,0,vp.texture.width, -float(vp.texture.height)), Rectangle(0,0,1000,1000), ::Vector2(0,0), 0.0f, WHITE);
+    DrawText(TextFormat("FPS: %d", GetFPS()), 0,0 , 40, GREEN);
+    for(int i = 0;i < 6;i++){
+        
+        GuiSlider(Rectangle(1100, i * 50 + 100, 200, 40), TextFormat("Actuator %d", i + 1), "", actuator_angles.data() + i, 0.0f, 4.0f * M_PI);
+    }
+    constexpr const char* names[] = {"X", "Y", "Z", "Roll", "Pitch", "Yaw"};
+    bool changed = false;
+    
+    auto aa_backup = actuator_angles;
+    for(int i = 0;i < 3;i++){
+        //float x = 0;
+        //std::cout << current_transform.translation(i).value << ", ";
+        double pp = current_transform.translation(i).value;
+        double pv = current_transform.euler_rotation_angles(i).value;
+        GuiSlider(Rectangle(1100, i * 50 + 500, 200, 40), names[i + 0], "", &current_transform.translation(i).value, -1.0f, 1.0f);
+        GuiSlider(Rectangle(1100, i * 50 + 700, 200, 40), names[i + 3], "", &current_transform.euler_rotation_angles(i).value, -0.5f, 0.5f);
+        if(std::abs(current_transform.translation(i).value - pp) > 1e-5){
+            changed = true;
+        }
+        if(std::abs(current_transform.euler_rotation_angles(i).value - pv) > 1e-5){
+            changed = true;
+        }
+    }
+    if (GuiButton(Rectangle(1100, 55, 200, 40), "Reset")){
+        changed = false;
+        actuator_angles.fill(0);
+    }
+    if(changed){
+        //std::cout << "Changed\n";
+        
+        for(int i = 0;i < 20;i++){
+            auto[v, J] = evaluate_with_jacobian_wrt_actuators<double>(current_transform, unit_hexagon, anchors);
+            FullPivHouseholderQR<Matrix6<double>> qr(J);
+
+            Vector6<double> subtract = qr.solve(v);
+            
+            actuator_angles -= subtract.cast<float>();
+            anchors = generate_hexagon(jet_type<scalar>(1.6), Vector3<jet_type<scalar>>(0, 0, -1.0));
+            for(int j = 0;j < 6;j++){
+                jet_type<double> actuator_angle_jet(actuator_angles[j]);
+                actuator_angle_jet.deriv(j + 6) = 1;
+                anchors.col(j) += (jet_type<double>(rad) * cos(actuator_angle_jet) * rotary_planes[j].first .cast<jet_type<double>>())
+                               +  (jet_type<double>(rad) * sin(actuator_angle_jet) * rotary_planes[j].second.cast<jet_type<double>>());
+            }
+            //std::cout << v << "\n";
+            if(i == 19){
+                auto[vi, Ji] = evaluate_with_jacobian_wrt_actuators<double>(current_transform, unit_hexagon, anchors);
+                if(vi.norm() > 0.1){
+                    actuator_angles = aa_backup;
+                }
+            }
+            //std::cout << anchors << "\n";
+
+        }
+        for(int j = 0;j < 6;j++){
+            actuator_angles(j) = std::fmod(actuator_angles(j) + 2.0 * M_PI, 2.0 * M_PI);
+        }
+    }
+    EndDrawing();
+}
+int main(){
+    InitWindow(1500, 1000, "Simulator");
+    rlSetLineWidth(5.0f);
+    
+    vp = LoadRenderTexture(4000, 4000);
+    
+    
+    GuiSetStyle(DEFAULT, TEXT_SIZE, 30);
+    GuiSetStyle(DEFAULT, TEXT_COLOR_NORMAL, ColorToInt(Color{100,255,100,255}));
+    GuiSetStyle(DEFAULT, TEXT_SPACING, 4.0f);
+    SetTargetFPS(60);
+    rlDisableBackfaceCulling();
+    #ifdef __EMSCRIPTEN__
+    emscripten_set_main_loop(gameLoop, 0, 0);
+    #else
+    while(!WindowShouldClose()){
+        gameLoop();
+    }
+    #endif
+    
+    
+    
+    //std::cout << current_guess.translation << "\n";
+    //std::cout << current_guess.euler_rotation_angles << "\n\n";
+    //for(int i = 0;i < 6;i++)
+    //    std::cout << (current_guess.apply(unit_hexagon).col(i).cast<double>() - anchors.col(i).cast<double>()).norm() << "\n";
+    //auto[v, J] = evaluate_with_jacobian<double>(current_guess, unit_hexagon, anchors);
+}

web/minshell.html

@@ -0,0 +1,87 @@
+<!doctype html>
+<html lang="en-us">
+  <head>
+    <meta charset="utf-8">
+    <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+    
+    <title>CV</title>
+    
+    <meta name="title" content="CV">
+    <meta name="description" content="A template for raylib games">
+    <meta name="keywords" content="programming, C, C++, library, CV, rendering">
+    <meta name="viewport" content="width=1500, user-scalable=yes">
+    <script src=headersetter.js></script>
+    <!-- Open Graph metatags for sharing -->
+    <meta property="og:title" content="CV">
+    <meta property="og:image:type" content="image/png">
+    <meta property="og:image" content="https://manuel5975p.github.io/cvlogo.png">
+    <meta property="og:site_name" content="manuel5975p.github.io">
+    <meta property="og:url" content="https://manuel5975p.github.io">
+    <meta property="og:description" content="CV">
+
+    <!-- Twitter metatags for sharing 
+    <meta name="twitter:card" content="summary">
+    <meta name="twitter:site" content="@raysan5">
+    <meta name="twitter:title" content="raylib web game">
+    <meta name="twitter:image" content="https://www.raylib.com/common/raylib_logo.png">
+    <meta name="twitter:url" content="https://www.raylib.com/games.html">
+    <meta name="twitter:description" content="New raylib web game, developed using raylib videogames library">-->
+    
+    <!-- Favicon -->
+    <link rel="shortcut icon" href="https://manuel5975p.github.io/favicon.ico">
+    
+    <!-- body { margin: 0px; } -->
+    <style>
+        html, body, div, canvas {
+            margin: 0;
+            padding: 0;
+        }
+        canvas.emscripten { border: 0px none; background-color: black; }
+    </style>
+    <script type='text/javascript' src="https://cdn.jsdelivr.net/gh/eligrey/FileSaver.js/dist/FileSaver.min.js"> </script>
+    <script type='text/javascript'>
+        function saveFileFromMEMFSToDisk(memoryFSname, localFSname)     // This can be called by C/C++ code
+        {
+            var isSafari = false; // Not supported, navigator.userAgent access is being restricted
+            //var isSafari = /^((?!chrome|android).)*safari/i.test(navigator.userAgent);
+            var data = FS.readFile(memoryFSname);
+            var blob;
+
+            if (isSafari) blob = new Blob([data.buffer], { type: "application/octet-stream" });
+            else blob = new Blob([data.buffer], { type: "application/octet-binary" });
+
+            // NOTE: SaveAsDialog is a browser setting. For example, in Google Chrome,
+            // in Settings/Advanced/Downloads section you have a setting:
+            // 'Ask where to save each file before downloading' - which you can set true/false.
+            // If you enable this setting it would always ask you and bring the SaveAsDialog
+            saveAs(blob, localFSname);
+        }
+    </script>
+    </head>
+    <body style="background-color:black;">
+        <canvas class=emscripten id=canvas oncontextmenu=event.preventDefault() tabindex=-1 crossorigin></canvas>
+        <p id="output" />
+        <script>
+            var Module = {
+                print: (function() {
+                    var element = document.getElementById('output');
+                    if (element) element.value = ''; // clear browser cache
+                    return function(text) {
+                        if (arguments.length > 1) text = Array.prototype.slice.call(arguments).join(' ');
+                        console.log(text);
+                        if (element) {
+                          element.value += text + "\n";
+                          element.scrollTop = element.scrollHeight; // focus on bottom
+                        }
+                    };
+                })(),            
+                canvas: (function() {
+                    var canvas = document.getElementById('canvas');
+                    return canvas;
+                })()
+            };
+        </script>
+        {{{ SCRIPT }}}
+    </body>
+</html>
+