rotation stuff

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))
 

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}$$
+$$m_{y} = \frac{w\sin(yaw)}{2}$$
+## Rotation