Inverse Kinematics

InverseKinematics/InverseKinematics.hpp

@@ -0,0 +1,127 @@
+#include "SynGame.hpp"
+
+class InverseKinematics
+{
+    private:
+        Vector2 root;
+        Vector2 *target;
+        Vector2 *points;
+        float *lengths;
+        float totalLenght;
+        unsigned int pointCount;
+        unsigned int iterationCount;
+        float distanceThreeshold; 
+    public:
+        InverseKinematics();
+        void SetPoints(Vector2 *, unsigned int);
+        void SetRoot(Vector2);
+        void SetTarget(Vector2 *);
+        void SetIterationCount(unsigned int);
+        void Iterate();
+        ~InverseKinematics();
+};
+
+InverseKinematics::InverseKinematics()
+{
+    points = NULL;
+    lengths = NULL;
+    pointCount = 0;
+    iterationCount = 0;
+    distanceThreeshold = 1;
+}
+
+void InverseKinematics::SetPoints(Vector2 *points, unsigned int count)
+{
+    int i;
+
+    this -> points = points;
+    this -> pointCount = count;
+    this -> lengths = new float[count - 1];
+    this -> totalLenght = 0.0;
+    for (i = 0; i < count - 1; i++)
+    {
+        *(this -> lengths + i) = (*(points + i) - *(points + i + 1)).Magnitude();
+        this -> totalLenght += *(this -> lengths + i);
+    }
+}
+
+void InverseKinematics::SetIterationCount(unsigned int count)
+{
+    if(count == 0)  return;
+    if(count > 100) return;
+
+    this -> iterationCount = count;
+}
+
+void InverseKinematics::SetTarget(Vector2 *target)
+{
+    this -> target = target;
+}
+
+void InverseKinematics::SetRoot(Vector2 root)
+{
+    this -> root = root;
+}
+
+void InverseKinematics::Iterate()
+{
+    int i;
+    Vector2 *point;
+    Vector2 *nextPoint;
+    float *length;
+    int iterationCount = this -> iterationCount;
+    int pointCount = this -> pointCount;
+    
+    // if the target is out of reach
+    if(target->Distance(root) > this -> totalLenght)
+    {
+        Vector2 direction = (*target - root).Normalized();
+        *(this -> points) = root;
+        point = this -> points + 1;
+        length = this -> lengths;
+
+        for (i = 1; i < pointCount; i++)
+        {
+            *point = *(point - 1) + direction * *length;
+            point++;
+            length++;
+        }
+        
+        return;
+    }
+
+    // if the target is in reachable distance
+    for (i = 0; i < iterationCount; i++)
+    {
+        point = this -> points + pointCount - 1;
+        if(point -> Distance(*target) < distanceThreeshold)
+            return;
+        length = this -> lengths + pointCount - 2;
+
+        *point = *target;
+
+        while (point != this -> points)
+        {
+            nextPoint = point - 1;
+            *nextPoint = (*point) + (*nextPoint - *point).Normalized() * (*length);
+            length--;
+            point--;
+        }
+
+        *point = root;
+        length = this -> lengths;
+
+        while (point != this -> points + pointCount - 1)
+        {
+            nextPoint = point + 1;
+            *nextPoint = (*point) + (*nextPoint - *point).Normalized() * (*length);
+            length++;
+            point++;
+        }
+    }
+}
+
+InverseKinematics::~InverseKinematics()
+{
+    if(lengths) delete lengths;
+}

InverseKinematics/SynGame.hpp

@@ -0,0 +1,15 @@
+#ifndef SynClasses
+    #define SynClasses
+
+    #define DegToRad 0.0174533 
+    
+    #include <SFML/Graphics.hpp>
+    #include <sstream>
+    #include <vector>
+    #include <iostream>
+    #include <cmath>
+    #include "Vectors.hpp"
+    #include "InverseKinematics.hpp"
+    #include "Window.hpp"
+    #include "TestWindow.hpp"
+#endif

InverseKinematics/TestWindow.hpp

@@ -0,0 +1,48 @@
+#include "SynGame.hpp"
+
+class TestWindow : public Window
+{
+    protected:
+        InverseKinematics kinematics;
+        Vector2 target;
+        Vector2 *points;
+        unsigned int pointCount;
+    public:
+        TestWindow(unsigned int = 960, unsigned int = 540, std::string = "Window", sf::Uint32 = sf::Style::Titlebar | sf::Style::Close);
+        void Update();
+};
+
+void TestWindow::Update()
+{
+    int i;
+    Window::Update();
+    
+    if(!isFocused)
+        return;
+
+    target = Vector2(sf::Mouse::getPosition(window).x, sf::Mouse::getPosition(window).y);
+    kinematics.Iterate();
+    sf::Vertex *array = new sf::Vertex[this -> pointCount];
+    
+    for (i = 0; i < this -> pointCount; i++)
+        *(array + i) = sf::Vertex(sf::Vector2f((points + i) -> x, (points + i) -> y), sf::Color::White);
+
+    window.clear(sf::Color::Black);
+    window.draw(array, this -> pointCount, sf::PrimitiveType::LinesStrip);
+    window.display();
+    delete array;
+}
+
+TestWindow::TestWindow(unsigned int width, unsigned int height, std::string title, sf::Uint32 style) : Window(width, height, title, style) 
+{
+    pointCount = 5;
+    points = new Vector2[pointCount];
+
+    for (int i = 0; i < pointCount; i++)
+        *(points + i) = Vector2(500 + i * 75, 500 + i * 75);
+
+    kinematics.SetPoints(points, pointCount);
+    kinematics.SetRoot(*(points + 0));
+    kinematics.SetIterationCount(10);
+    kinematics.SetTarget(&target);
+}

InverseKinematics/Vectors.hpp

@@ -0,0 +1,157 @@
+#include "SynGame.hpp"
+
+#pragma region Vector1
+    class Vector1
+    {
+        public:
+            float x;
+            Vector1(float = 0);
+            Vector1 operator+(Vector1);
+            Vector1 operator-(Vector1);
+            Vector1 operator/(Vector1);
+            Vector1 operator*(Vector1);
+            Vector1 operator+=(Vector1);
+            Vector1 operator-=(Vector1);
+            Vector1 operator/=(Vector1);
+            Vector1 operator*=(Vector1);
+            Vector1 operator/(float);
+            Vector1 operator*(float);
+            Vector1 operator/=(float);
+            Vector1 operator*=(float);
+            float Magnitude();
+            float Distance(Vector1);
+            Vector1 Normalized();
+    };
+
+    Vector1::Vector1(float x) { this -> x = x; }
+
+    Vector1 Vector1::operator+ (Vector1 parameter) { Vector1 vector = Vector1(x); vector.x += parameter.x; return vector; }
+    Vector1 Vector1::operator- (Vector1 parameter) { Vector1 vector = Vector1(x); vector.x -= parameter.x; return vector; }
+    Vector1 Vector1::operator/ (Vector1 parameter) { Vector1 vector = Vector1(x); vector.x /= parameter.x; return vector; }
+    Vector1 Vector1::operator* (Vector1 parameter) { Vector1 vector = Vector1(x); vector.x *= parameter.x; return vector; }
+    Vector1 Vector1::operator+=(Vector1 parameter) { x += parameter.x; return *this; }
+    Vector1 Vector1::operator-=(Vector1 parameter) { x -= parameter.x; return *this; }
+    Vector1 Vector1::operator/=(Vector1 parameter) { x /= parameter.x; return *this; }
+    Vector1 Vector1::operator*=(Vector1 parameter) { x *= parameter.x; return *this; }
+    
+    Vector1 Vector1::operator/ (float parameter) { Vector1 vector = Vector1(x); vector.x /= parameter; return vector; }
+    Vector1 Vector1::operator* (float parameter) { Vector1 vector = Vector1(x); vector.x *= parameter; return vector; }
+    Vector1 Vector1::operator/=(float parameter) { x /= parameter; return *this; }
+    Vector1 Vector1::operator*=(float parameter) { x *= parameter; return *this; }
+
+    float Vector1::Magnitude() { return x; }
+    float Vector1::Distance(Vector1 parameter) 
+    { 
+        Vector1 distanceVector = (*this) - parameter;
+        return distanceVector.Magnitude(); 
+    }
+    Vector1 Vector1::Normalized() { return Vector1(x < 0.0 ? -1.0 : 1.0); }
+#pragma endregion
+#pragma region Vector2
+    class Vector2
+    {
+        public:
+            float x;
+            float y;
+            Vector2(float = 0, float = 0);
+            Vector2 operator+(Vector2);
+            Vector2 operator-(Vector2);
+            Vector2 operator/(Vector2);
+            Vector2 operator*(Vector2);
+            Vector2 operator+=(Vector2);
+            Vector2 operator-=(Vector2);
+            Vector2 operator/=(Vector2);
+            Vector2 operator*=(Vector2);
+            Vector2 operator/(float);
+            Vector2 operator*(float);
+            Vector2 operator/=(float);
+            Vector2 operator*=(float);
+            float Magnitude();
+            float Distance(Vector2);
+            Vector2 Normalized();
+    };
+
+    Vector2::Vector2(float x, float y) { this -> x = x; this -> y = y; }
+
+    Vector2 Vector2::operator+ (Vector2 parameter) { Vector2 vector = Vector2(x, y); vector.x += parameter.x; vector.y += parameter.y; return vector; }
+    Vector2 Vector2::operator- (Vector2 parameter) { Vector2 vector = Vector2(x, y); vector.x -= parameter.x; vector.y -= parameter.y; return vector; }
+    Vector2 Vector2::operator/ (Vector2 parameter) { Vector2 vector = Vector2(x, y); vector.x /= parameter.x; vector.y /= parameter.y; return vector; }
+    Vector2 Vector2::operator* (Vector2 parameter) { Vector2 vector = Vector2(x, y); vector.x *= parameter.x; vector.y *= parameter.y; return vector; }
+    Vector2 Vector2::operator+=(Vector2 parameter) { x += parameter.x; y += parameter.y; return *this; }
+    Vector2 Vector2::operator-=(Vector2 parameter) { x -= parameter.x; y -= parameter.y; return *this; }
+    Vector2 Vector2::operator/=(Vector2 parameter) { x /= parameter.x; y /= parameter.y; return *this; }
+    Vector2 Vector2::operator*=(Vector2 parameter) { x *= parameter.x; y *= parameter.y; return *this; }
+
+    Vector2 Vector2::operator/ (float parameter) { Vector2 vector = Vector2(x, y); vector.x /= parameter; vector.y /= parameter; return vector; }
+    Vector2 Vector2::operator* (float parameter) { Vector2 vector = Vector2(x, y); vector.x *= parameter; vector.y *= parameter; return vector; }
+    Vector2 Vector2::operator/=(float parameter) { x *= parameter; y *= parameter; return *this; }
+    Vector2 Vector2::operator*=(float parameter) { x /= parameter; y /= parameter; return *this; }
+
+    float Vector2::Magnitude() { return sqrt(x*x+y*y); }
+    float Vector2::Distance(Vector2 parameter) 
+    { 
+        Vector2 distanceVector = (*this) - parameter;
+        return distanceVector.Magnitude(); 
+    }
+    Vector2 Vector2::Normalized() 
+    { 
+        Vector2 vector = Vector2(x, y);
+        return vector / Magnitude();
+    }
+#pragma endregion
+#pragma region Vector3
+    class Vector3
+    {
+        public:
+            float x;
+            float y;
+            float z;
+            Vector3(float = 0, float = 0, float = 0);
+            Vector3 operator+(Vector3);
+            Vector3 operator-(Vector3);
+            Vector3 operator/(Vector3);
+            Vector3 operator*(Vector3);
+            Vector3 operator+=(Vector3);
+            Vector3 operator-=(Vector3);
+            Vector3 operator/=(Vector3);
+            Vector3 operator*=(Vector3);
+            Vector3 operator/(float);
+            Vector3 operator*(float);
+            Vector3 operator/=(float);
+            Vector3 operator*=(float);
+            float Magnitude();
+            float Distance(Vector3);
+            Vector3 Normalized();
+    };
+
+    Vector3::Vector3(float x, float y, float z) { this -> x = x; this -> y = y; this -> z = z; }
+
+    Vector3 Vector3::operator+ (Vector3 parameter) { Vector3 vector = Vector3(x, y, z); vector.x += parameter.x; vector.y += parameter.y; vector.z += parameter.z; return vector; }
+    Vector3 Vector3::operator- (Vector3 parameter) { Vector3 vector = Vector3(x, y, z); vector.x -= parameter.x; vector.y -= parameter.y; vector.z -= parameter.z; return vector; }
+    Vector3 Vector3::operator/ (Vector3 parameter) { Vector3 vector = Vector3(x, y, z); vector.x /= parameter.x; vector.y /= parameter.y; vector.z /= parameter.z; return vector; }
+    Vector3 Vector3::operator* (Vector3 parameter) { Vector3 vector = Vector3(x, y, z); vector.x *= parameter.x; vector.y *= parameter.y; vector.z *= parameter.z; return vector; }
+    Vector3 Vector3::operator+=(Vector3 parameter) { x += parameter.x; y += parameter.y; z += parameter.z; return *this; }
+    Vector3 Vector3::operator-=(Vector3 parameter) { x -= parameter.x; y -= parameter.y; z -= parameter.z; return *this; }
+    Vector3 Vector3::operator/=(Vector3 parameter) { x /= parameter.x; y /= parameter.y; z /= parameter.z; return *this; }
+    Vector3 Vector3::operator*=(Vector3 parameter) { x *= parameter.x; y *= parameter.y; z *= parameter.z; return *this; }
+
+    Vector3 Vector3::operator/=(float parameter) { x *= parameter; y *= parameter; z *= parameter; return *this; }
+    Vector3 Vector3::operator*=(float parameter) { x /= parameter; y /= parameter; z /= parameter; return *this; }
+    Vector3 Vector3::operator/ (float parameter) { Vector3 vector = Vector3(x, y, z); vector.x /= parameter; vector.y /= parameter; vector.z /= parameter; return vector; }
+    Vector3 Vector3::operator* (float parameter) { Vector3 vector = Vector3(x, y, z); vector.x *= parameter; vector.y *= parameter; vector.z *= parameter; return vector; }
+
+    float Vector3::Magnitude() { return sqrt(x*x+y*y+z*z); }
+    float Vector3::Distance(Vector3 parameter) 
+    { 
+        Vector3 distanceVector;
+        distanceVector.x = x - parameter.x;
+        distanceVector.y = y - parameter.y;
+        distanceVector.z = z - parameter.z;
+        return distanceVector.Magnitude(); 
+    }
+    Vector3 Vector3::Normalized() 
+    { 
+        Vector3 vector = Vector3(x, y, z);
+        return vector / Magnitude();
+    }
+#pragma endregion

InverseKinematics/Window.hpp

@@ -0,0 +1,126 @@
+#include "SynGame.hpp"
+
+class Window
+{
+    protected:
+        sf::RenderWindow window;
+        sf::Event event;
+        sf::Vector2u size;
+        sf::Vector2u windowedSize;
+        std::string title;
+        sf::Uint32 style;
+        bool isFocused;
+        bool fullscreen;
+    public:
+        Window(unsigned int = 960, unsigned int = 540, std::string = "Window", sf::Uint32 = sf::Style::Titlebar | sf::Style::Close);
+        virtual void CreateWindow();
+        virtual void CloseWindow();
+        virtual void Update();
+        void SetFrameRate(int = 0);
+        void SetTitle(std::string);
+        virtual void SetSize(unsigned int, unsigned int);
+        bool IsOpen();
+};
+
+void Window::Update()
+{
+    while (window.pollEvent(event))
+    {
+        if (event.type == sf::Event::Closed)
+            CloseWindow();
+        else if (event.type == sf::Event::LostFocus)
+            isFocused = false;
+        else if (event.type == sf::Event::GainedFocus)
+            isFocused = true;
+        if(sf::Keyboard::isKeyPressed(sf::Keyboard::F))
+        {
+            fullscreen = !fullscreen;
+            CloseWindow();
+            CreateWindow();
+        }
+    }
+
+    if(!isFocused)
+        return;
+}
+
+Window::Window(unsigned int width, unsigned int height, std::string title, sf::Uint32 style)
+{
+    this -> size.x = width;
+    this -> size.y = height;
+    this -> title  = title;
+    this -> style  = style;
+
+    windowedSize.x = width;
+    windowedSize.y = height;
+    
+    isFocused = true;
+    fullscreen = false;
+    SetFrameRate();
+    CreateWindow();
+}
+
+void Window::CreateWindow()
+{
+    if(window.isOpen())
+        return;
+
+    if(!fullscreen)
+    {
+        size = windowedSize;
+        sf::VideoMode videoMode(windowedSize.x, windowedSize.y);
+        window.create(videoMode, title, style);
+    }
+    else
+    {
+        sf::VideoMode videoMode(sf::VideoMode::getDesktopMode());
+        size.x = videoMode.width;
+        size.y = videoMode.height;
+        window.create(videoMode, title, sf::Style::Fullscreen);
+    }
+}
+
+void Window::CloseWindow()
+{
+    if(!window.isOpen())
+        return;
+
+    window.close();
+}
+
+void Window::SetFrameRate(int rate)
+{
+    if(rate == 0)
+    {
+        window.setVerticalSyncEnabled(true);
+        return;
+    }
+    
+    if(rate < 0)
+        window.setFramerateLimit(10000);
+    else
+        window.setFramerateLimit(rate);
+
+    window.setVerticalSyncEnabled(false);
+}
+
+void Window::SetTitle(std::string title)
+{
+    this -> title = title;
+    window.setTitle(title);
+}
+
+void Window::SetSize(unsigned int width, unsigned int height)
+{
+    size = sf::Vector2u(width, height);
+    sf::Vector2i pos = window.getPosition();
+    CloseWindow();
+    window.setSize(size);
+    CreateWindow();
+    window.setPosition(pos);
+}
+
+bool Window::IsOpen()
+{
+    return window.isOpen();
+}

InverseKinematics/main.cpp

@@ -0,0 +1,12 @@
+#include "SynGame.hpp"
+
+int main()
+{
+    TestWindow window(1600, 900, "Inverse Kinematics Test Syntriax");
+    window.SetFrameRate(60);
+    
+    while (window.IsOpen())
+        window.Update();
+
+    return 0;
+}