From 3985d5386dd12173e8ec4eb908ef5466606fb9f7 Mon Sep 17 00:00:00 2001
From: DeDiamondPro <67508414+DeDiamondPro@users.noreply.github.com>
Date: Wed, 8 Nov 2023 09:25:27 +0100
Subject: [PATCH] rotation stuff

---
 code/inverse-kinematics-test/inverse_kinematics.py   | 10 ++++++++++
 documentation/src/ch04-02-inverse-kinematics-body.md |  5 ++++-
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/code/inverse-kinematics-test/inverse_kinematics.py b/code/inverse-kinematics-test/inverse_kinematics.py
index 02e2ecd..fda70b8 100644
--- a/code/inverse-kinematics-test/inverse_kinematics.py
+++ b/code/inverse-kinematics-test/inverse_kinematics.py
@@ -1,5 +1,13 @@
 from math import *
 
+
+# https://en.wikipedia.org/wiki/Rotation_matrix
+def calculate_rotation(x1, y1, angle):
+    x2 = x1 * cos(radians(angle)) - y1 * sin(radians(angle))
+    y2 = x1 * sin(radians(angle)) + y1 * cos(radians(angle))
+    return x2 - x1, y2 - y1
+
+
 pitch = 10.0
 yaw = 10.0
 l = 20.0
@@ -10,6 +18,8 @@
 leg_movement_x = l * (1 - cos(radians(pitch))) / 2
 leg_movement_y = b * (1 - cos(radians(yaw))) / 2
 
+print(calculate_rotation(1.0, 1.0, -30.0))
+
 print("Height difference pitch: {}, height difference yaw: {}".format(height_diff_pitch, height_diff_yaw))
 print("Leg movement x: {}, leg movement y: {}".format(leg_movement_x, leg_movement_y))
 
diff --git a/documentation/src/ch04-02-inverse-kinematics-body.md b/documentation/src/ch04-02-inverse-kinematics-body.md
index 180c724..5159e20 100644
--- a/documentation/src/ch04-02-inverse-kinematics-body.md
+++ b/documentation/src/ch04-02-inverse-kinematics-body.md
@@ -10,6 +10,8 @@
 
 \\(rotation\\) desired rotation
 
+\\(rotation\\) desired rotation
+
 \\(m_{x}\\) leg movement x
 
 \\(m_{y}\\) leg movement y
@@ -20,4 +22,5 @@ $$m_{x} = \frac{l(1-\cos(pitch))}{2}$$
 $$m_{y} = \frac{l\sin(pitch)}{2}$$
 ## Yaw
 $$m_{z} = \frac{w(1-\cos(yaw))}{2}$$
-$$m_{y} = \frac{w\sin(yaw)}{2}$$
\ No newline at end of file
+$$m_{y} = \frac{w\sin(yaw)}{2}$$
+## Rotation