4.2.1

### Question

``````extends Node3D

# Only inputs are linkage lengths, starting angle
# Later will add motor parameters.
@export var link1_length: float = 2
@export var link2_length: float = 2
@export var link3_length: float = 2
@export var link4_length: float = 2
@export var input_angle: float = 90

# Called when the node enters the scene tree for the first time.

## CALCULATE LINK POSITION AND ORIENTATIONS

var psi = PI - theta

var L = sqrt(A**2 + B**2 - 2 * A * B * cos(psi))

var alpha = acos((L**2 + B**2 - A**2) / (2 * L * B))
var beta = acos((L**2 + C**2 - D**2) / (2 * L * C))
var lambda = (PI-alpha-beta) + theta

var gamma = (PI - acos((C**2 + D**2 - L**2) / (2 * C * D))) + lambda

var pos0 = Vector3.ZERO
var pos1 = Vector3(A,0,0)
var pos2 = pos1 + Vector3(B * cos(theta),
B * sin(theta),
0)
var pos3 = pos2 + Vector3(C * cos(lambda),
C * sin(lambda),
0)
var pos4 = pos3 + Vector3(D * cos(gamma),
D * sin(gamma),
0)

var joint_positions = [pos0, pos1, pos2, pos3]
var link_angles = [0.0, theta, lambda, gamma]

# APPLYING LINK LENGTHS AND JOINT POSITION
var i = 0

if node is MeshInstance3D:
var mesh = node.get_mesh()

if mesh is CapsuleMesh:

if node is CollisionShape3D:
var shape = node.get_shape()

if node is HingeJoint3D: