Inverse Kinematics Completed

2025-02-25 12:36:13 -05:00 · 2025-02-25 12:36:13 -05:00 · cf6c61be32
commit cf6c61be32
parent 900e28c313
10 changed files with 393 additions and 2 deletions
--- a/BasicProjectionRenderer/BasicProjectionRenderer/Form1.cs
+++ b/BasicProjectionRenderer/BasicProjectionRenderer/Form1.cs
@ -114,6 +114,7 @@ public partial class Form1 : Form
    private void OnTick(object? sender, EventArgs e)
    {
        elapsedTime += RefreshTimer.Interval * 1e-3;
+        //temp += MathE.Sin(elapsedTime * 8) * 0.9;
        if (Mesh is not null)
        {
            DateTime start = DateTime.Now;
--- a/BasicProjectionRenderer/BasicProjectionRenderer/Program.cs
+++ b/BasicProjectionRenderer/BasicProjectionRenderer/Program.cs
@ -19,8 +19,8 @@ public static class Program
    {
        Application.SetCompatibleTextRenderingDefault(false);
        Application.SetHighDpiMode(HighDpiMode.PerMonitorV2);
-
-        string path = "cube.obj";
+        
+        string path = "monkey.obj";
        FileStream fs = new(path, FileMode.Open);
        Mesh obj = Mesh.FromObj(fs);
        fs.Close();
--- a/InverseKinematics/InverseKinematics.sln
+++ b/InverseKinematics/InverseKinematics.sln
@ -0,0 +1,22 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.12.35506.116 d17.12
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "InverseKinematics", "InverseKinematics\InverseKinematics.csproj", "{74875265-97D7-42EA-A2F2-23D2A890FD85}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{74875265-97D7-42EA-A2F2-23D2A890FD85}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{74875265-97D7-42EA-A2F2-23D2A890FD85}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{74875265-97D7-42EA-A2F2-23D2A890FD85}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{74875265-97D7-42EA-A2F2-23D2A890FD85}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+EndGlobal
--- a/InverseKinematics/InverseKinematics/Form1.Designer.cs
+++ b/InverseKinematics/InverseKinematics/Form1.Designer.cs
@ -0,0 +1,45 @@
+namespace InverseKinematics
+{
+    partial class Form1
+    {
+        /// <summary>
+        ///  Required designer variable.
+        /// </summary>
+        private System.ComponentModel.IContainer components = null;
+
+        /// <summary>
+        ///  Clean up any resources being used.
+        /// </summary>
+        /// <param name="disposing">true if managed resources should be disposed; otherwise, false.</param>
+        protected override void Dispose(bool disposing)
+        {
+            if (disposing && (components != null))
+            {
+                components.Dispose();
+            }
+            base.Dispose(disposing);
+        }
+
+        #region Windows Form Designer generated code
+
+        /// <summary>
+        ///  Required method for Designer support - do not modify
+        ///  the contents of this method with the code editor.
+        /// </summary>
+        private void InitializeComponent()
+        {
+            SuspendLayout();
+            // 
+            // Form1
+            // 
+            AutoScaleDimensions = new SizeF(13F, 32F);
+            AutoScaleMode = AutoScaleMode.Font;
+            ClientSize = new Size(1134, 720);
+            Name = "Form1";
+            Text = "Form1";
+            ResumeLayout(false);
+        }
+
+        #endregion
+    }
+}
--- a/InverseKinematics/InverseKinematics/Form1.cs
+++ b/InverseKinematics/InverseKinematics/Form1.cs
@ -0,0 +1,155 @@
+using Nerd_STF.Mathematics;
+using System.Drawing.Drawing2D;
+
+namespace InverseKinematics;
+
+public partial class Form1 : Form
+{
+    public Float2 DesiredPoint { get; set; }
+
+    public Float2[] Points { get; set; }
+
+    public Float2 ZoomLevel { get; set; } = (3, 3);
+    public Float2 ScreenCenter { get; set; } = (0, 0);
+
+    public Form1()
+    {
+        InitializeComponent();
+        Points = new Float2[5];
+        for (int i = 0; i < Points.Length; i++)
+        {
+            Points[i] = (0, i);
+        }
+
+        SetStyle(ControlStyles.UserPaint, true);
+        SetStyle(ControlStyles.AllPaintingInWmPaint, true);
+        SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
+    }
+
+    public Int2 ToScreenSpace(Float2 worldPoint)
+    {
+        float dpi = DeviceDpi;
+        worldPoint.y = -worldPoint.y;
+
+        worldPoint.x -= ScreenCenter.x;
+        worldPoint.y -= ScreenCenter.y;
+
+        worldPoint.x *= dpi / ZoomLevel.x;
+        worldPoint.y *= dpi / ZoomLevel.y;
+
+        worldPoint.x += ClientRectangle.Width * 0.5;
+        worldPoint.y += ClientRectangle.Height * 0.5;
+
+        return new((int)worldPoint.x, (int)worldPoint.y);
+    }
+    public Float2 FromScreenSpace(Int2 screenPoint)
+    {
+        Float2 result = (screenPoint.x, screenPoint.y);
+
+        result.x -= ClientRectangle.Width / 2.0;
+        result.y -= ClientRectangle.Height / 2.0;
+
+        float dpi = DeviceDpi;
+        result.x /= dpi / ZoomLevel.x;
+        result.y /= dpi / ZoomLevel.y;
+
+        result.x += ScreenCenter.x;
+        result.y += ScreenCenter.y;
+
+        result.y = -result.y;
+
+        return result;
+    }
+
+    protected override void OnMouseMove(MouseEventArgs e)
+    {
+        DesiredPoint = FromScreenSpace(e.Location);
+        Invalidate();
+    }
+    protected override void OnPaint(PaintEventArgs e)
+    {
+        RunFabrik();
+
+        Graphics g = e.Graphics;
+        g.SmoothingMode = SmoothingMode.HighQuality;
+        float scaleFactor = g.DpiX / 96.0f;
+
+        SolidBrush brush = new(Color.Black);
+        Pen pen = new(Color.Black, scaleFactor);
+
+        Float2 radius = (2, 2);
+        Int2 prevPointPix = (0, 0);
+        for (int i = 0; i < Points.Length; i++)
+        {
+            Int2 pointPix = ToScreenSpace(Points[i]);
+            g.FillEllipse(brush, new RectangleF(pointPix - radius * scaleFactor, radius * scaleFactor * 2));
+        
+            if (i > 0)
+            {
+                g.DrawLine(pen, prevPointPix, pointPix);
+            }
+            prevPointPix = pointPix;
+        }
+
+        Int2 desiredPix = ToScreenSpace(DesiredPoint);
+        radius = (5, 5);
+
+        brush.Color = Color.Red;
+        g.FillEllipse(brush, new RectangleF(desiredPix - radius * scaleFactor, radius * scaleFactor * 2));
+    }
+
+    private void RunFabrik()
+    {
+        // Initial setup.
+        for (int i = 0; i < Points.Length; i++)
+        {
+            // Reset points to 90 degrees away from goal.
+            // This produces a "nice" rotation no matter where
+            // you are.
+            Float2 dir = (-DesiredPoint.y, DesiredPoint.x);
+            dir.Normalize();
+
+            Points[i] = dir * i;
+        }
+
+        RunFabrik(true, 16);
+    }
+
+    // True = forwards, false = backwards.
+    private void RunFabrik(bool direction, int maxIters)
+    {
+        if (maxIters <= 0) return;
+
+        const double maxLength = 1;
+        if (direction)
+        {
+            Points[^1] = DesiredPoint;
+            for (int i = Points.Length - 2; i >= 0; i--)
+            {
+                Float2 thisPoint = Points[i],
+                       connected = Points[i + 1],
+                       diff = connected - thisPoint;
+
+                double step = maxLength - diff.Magnitude;
+                Float2 dir = diff.Normalized;
+                Points[i] = thisPoint - dir * step;
+            }
+        }
+        else
+        {
+            Points[0] = (0, 0);
+            for (int i = 1; i < Points.Length; i++)
+            {
+                Float2 thisPoint = Points[i],
+                       connected = Points[i - 1],
+                       diff = connected - thisPoint;
+
+                double step = maxLength - diff.Magnitude;
+                Float2 dir = diff.Normalized;
+                Points[i] = thisPoint - dir * step;
+            }
+        }
+
+        RunFabrik(!direction, maxIters - 1);
+    }
+}
--- a/InverseKinematics/InverseKinematics/Form1.resx
+++ b/InverseKinematics/InverseKinematics/Form1.resx
@ -0,0 +1,120 @@
+<?xml version="1.0" encoding="utf-8"?>
+<root>
+  <!--
+    Microsoft ResX Schema
+
+    Version 2.0
+
+    The primary goals of this format is to allow a simple XML format
+    that is mostly human readable. The generation and parsing of the
+    various data types are done through the TypeConverter classes
+    associated with the data types.
+
+    Example:
+
+    ... ado.net/XML headers & schema ...
+    <resheader name="resmimetype">text/microsoft-resx</resheader>
+    <resheader name="version">2.0</resheader>
+    <resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
+    <resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
+    <data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
+    <data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
+    <data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
+        <value>[base64 mime encoded serialized .NET Framework object]</value>
+    </data>
+    <data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
+        <value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
+        <comment>This is a comment</comment>
+    </data>
+
+    There are any number of "resheader" rows that contain simple
+    name/value pairs.
+
+    Each data row contains a name, and value. The row also contains a
+    type or mimetype. Type corresponds to a .NET class that support
+    text/value conversion through the TypeConverter architecture.
+    Classes that don't support this are serialized and stored with the
+    mimetype set.
+
+    The mimetype is used for serialized objects, and tells the
+    ResXResourceReader how to depersist the object. This is currently not
+    extensible. For a given mimetype the value must be set accordingly:
+
+    Note - application/x-microsoft.net.object.binary.base64 is the format
+    that the ResXResourceWriter will generate, however the reader can
+    read any of the formats listed below.
+
+    mimetype: application/x-microsoft.net.object.binary.base64
+    value   : The object must be serialized with
+            : System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
+            : and then encoded with base64 encoding.
+
+    mimetype: application/x-microsoft.net.object.soap.base64
+    value   : The object must be serialized with
+            : System.Runtime.Serialization.Formatters.Soap.SoapFormatter
+            : and then encoded with base64 encoding.
+
+    mimetype: application/x-microsoft.net.object.bytearray.base64
+    value   : The object must be serialized into a byte array
+            : using a System.ComponentModel.TypeConverter
+            : and then encoded with base64 encoding.
+    -->
+  <xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
+    <xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
+    <xsd:element name="root" msdata:IsDataSet="true">
+      <xsd:complexType>
+        <xsd:choice maxOccurs="unbounded">
+          <xsd:element name="metadata">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" />
+              </xsd:sequence>
+              <xsd:attribute name="name" use="required" type="xsd:string" />
+              <xsd:attribute name="type" type="xsd:string" />
+              <xsd:attribute name="mimetype" type="xsd:string" />
+              <xsd:attribute ref="xml:space" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="assembly">
+            <xsd:complexType>
+              <xsd:attribute name="alias" type="xsd:string" />
+              <xsd:attribute name="name" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="data">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+                <xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
+              <xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
+              <xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
+              <xsd:attribute ref="xml:space" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="resheader">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" />
+            </xsd:complexType>
+          </xsd:element>
+        </xsd:choice>
+      </xsd:complexType>
+    </xsd:element>
+  </xsd:schema>
+  <resheader name="resmimetype">
+    <value>text/microsoft-resx</value>
+  </resheader>
+  <resheader name="version">
+    <value>2.0</value>
+  </resheader>
+  <resheader name="reader">
+    <value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+  <resheader name="writer">
+    <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+</root>
--- a/InverseKinematics/InverseKinematics/InverseKinematics.csproj
+++ b/InverseKinematics/InverseKinematics/InverseKinematics.csproj
@ -0,0 +1,15 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <OutputType>WinExe</OutputType>
+    <TargetFramework>net8.0-windows</TargetFramework>
+    <Nullable>enable</Nullable>
+    <UseWindowsForms>true</UseWindowsForms>
+    <ImplicitUsings>enable</ImplicitUsings>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="Nerd_STF" Version="3.0.0-beta3" />
+  </ItemGroup>
+
+</Project>
--- a/InverseKinematics/InverseKinematics/InverseKinematics.csproj.user
+++ b/InverseKinematics/InverseKinematics/InverseKinematics.csproj.user
@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+    <ItemGroup>
+        <Compile Update="Form1.cs">
+            <SubType>Form</SubType>
+        </Compile>
+    </ItemGroup>
+</Project>
--- a/InverseKinematics/InverseKinematics/Program.cs
+++ b/InverseKinematics/InverseKinematics/Program.cs
@ -0,0 +1,20 @@
+/**********722871**********
+ * Date: 2/25/2025
+ * Programmer: Kyle Gilbert
+ * Program Name: InverseKinematics
+ * Program Description: Calculates a series of joints with inverse kinematics.
+ **************************/
+
+namespace InverseKinematics;
+
+internal static class Program
+{
+    [STAThread]
+    public static void Main()
+    {
+        Application.EnableVisualStyles();
+        Application.SetCompatibleTextRenderingDefault(false);
+        Application.SetHighDpiMode(HighDpiMode.SystemAware);
+        Application.Run(new Form1());
+    }
+}
--- a/README.md
+++ b/README.md
@ -50,3 +50,8 @@ I have about 1-2 weeks for each project. Check the Git commits for specific date
 	- Not super optimized, but I've made a few tweaks to speed it up.
 	- All calculations are my own, from sin and cosine to matrix multiplication.
 	- Math code comes from a library I wrote, Nerd_STF.
+- InverseKinematics/
+    - A program that calculates inverse kinematics for a series of joints.
+    - Took me less than an hour to write. Faster than I expected.
+    - Handles hundreds of nodes with very little optimization.
+    - Might improve in the future to work around obstacles.