From c0240e6471b2923341670669f7bc9b452f183ad4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Asr=C4=B1n=20Do=C4=9Fan?= Date: Fri, 8 May 2020 23:28:32 +0300 Subject: [PATCH] Inverse Kinematics --- InverseKinematics/InverseKinematics.hpp | 127 +++++++++++++++++++ InverseKinematics/SynGame.hpp | 15 +++ InverseKinematics/TestWindow.hpp | 48 ++++++++ InverseKinematics/Vectors.hpp | 157 ++++++++++++++++++++++++ InverseKinematics/Window.hpp | 126 +++++++++++++++++++ InverseKinematics/main.cpp | 12 ++ 6 files changed, 485 insertions(+) create mode 100644 InverseKinematics/InverseKinematics.hpp create mode 100644 InverseKinematics/SynGame.hpp create mode 100644 InverseKinematics/TestWindow.hpp create mode 100644 InverseKinematics/Vectors.hpp create mode 100644 InverseKinematics/Window.hpp create mode 100644 InverseKinematics/main.cpp diff --git a/InverseKinematics/InverseKinematics.hpp b/InverseKinematics/InverseKinematics.hpp new file mode 100644 index 0000000..62808e3 --- /dev/null +++ b/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; +} diff --git a/InverseKinematics/SynGame.hpp b/InverseKinematics/SynGame.hpp new file mode 100644 index 0000000..1455a70 --- /dev/null +++ b/InverseKinematics/SynGame.hpp @@ -0,0 +1,15 @@ +#ifndef SynClasses + #define SynClasses + + #define DegToRad 0.0174533 + + #include + #include + #include + #include + #include + #include "Vectors.hpp" + #include "InverseKinematics.hpp" + #include "Window.hpp" + #include "TestWindow.hpp" +#endif diff --git a/InverseKinematics/TestWindow.hpp b/InverseKinematics/TestWindow.hpp new file mode 100644 index 0000000..2d0ae41 --- /dev/null +++ b/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); +} diff --git a/InverseKinematics/Vectors.hpp b/InverseKinematics/Vectors.hpp new file mode 100644 index 0000000..fe0df76 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/InverseKinematics/Window.hpp b/InverseKinematics/Window.hpp new file mode 100644 index 0000000..4f2c8c8 --- /dev/null +++ b/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(); +} diff --git a/InverseKinematics/main.cpp b/InverseKinematics/main.cpp new file mode 100644 index 0000000..ea3906d --- /dev/null +++ b/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; +}