#VRML_SIM R2025a utf8
# Articulated Joint for the course PN3002 Robotica
# Updated 2026-02-11

PROTO JointDH [
  field SFString joint        "1"
  field SFBool   rotation     TRUE
  field SFFloat  alpha        0.0
  field SFFloat  a            0.4
  field SFFloat  d            0.6
  field SFFloat  theta        0.0
  field SFFloat  position     0.0
  field SFFloat  minPosition  0.0
  field SFFloat  maxPosition  0.0
  field MFNode   nextJoints   []
]
{
  %<
  let name = fields.joint.value;
  let type = fields.rotation.value;
  let theta = fields.theta.value;
  let d = fields.d.value;
  let a = fields.a.value;
  let alpha = fields.alpha.value;
  let lengthA = Math.abs(a);
  let lengthD = Math.abs(d);
  >%

  %< if(type){ >%
  HingeJoint {
    jointParameters HingeJointParameters {
      position IS position
      minStop IS minPosition
      maxStop IS maxPosition
      axis 0 0 1
    }
    device [
      RotationalMotor {
        name "joint_%<=name>%"
        minPosition IS minPosition
        maxPosition IS maxPosition
      }
      PositionSensor {
        name "sensor_%<=name>%"
      }
    ]
  %<}else{>%
  SliderJoint {
    jointParameters JointParameters {
      position IS position
      minStop IS minPosition
      maxStop IS maxPosition
    }
    device [
      LinearMotor {
        name "joint_%<=name>%"
        minPosition IS minPosition
        maxPosition IS maxPosition
        maxVelocity 100
        maxForce 100
      }
      PositionSensor {
        name "sensor_%<=name>%"
      }
    ]
  %<}>%
    endPoint Solid {
      name "link_%<=name>%"
      children [
        DEF GEOMETRY%<=name>% Group {
          children [
            %< if(lengthA>0){ >%
            DEF A%<=name>% Pose {
              translation 0 0 0
              rotation 0 0 1 %<= theta >%
              children [
              DEF B%<=name>% Pose {
                  translation %<= a/2 >% 0 %<= d >%
                  rotation 0 1 0 1.5708
                  children [
                    Shape {
                      appearance PBRAppearance {
                        roughness 1
                        metalness 0
                      }
                      geometry Cylinder {
                        height %<= lengthA >%
                        radius 0.02
                      }
                    }
                  ]
                }
              ]
            }
            %<}>%
            DEF C%<=name>% Pose {
              translation 0 0 %<= d >%
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  geometry Sphere {
                    radius 0.02
                  }
                }
              ]
            }
            %< if(lengthD>0){ >%
            DEF D%<=name>% Pose {
              translation 0 0 %<= d/2 >%
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  geometry Cylinder {
                    height %<= lengthD >%
                    radius 0.02
                  }
                }
              ]
            }
            %<}>%
            DEF E%<=name>% Pose {
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  %< if(type){ >%
                  geometry Cylinder {
                    height 0.1
                    radius 0.05
                  }
                  %<}else{>%
                  geometry Box {
                    size 0.1 0.1 0.1
                  }
                  %<}>%
                }
              ]
            }
          ]
        }
        Pose {
          translation 0 0 0
          rotation 0 0 1 %<= theta >%
          children [
            Pose {
              translation %<= a >% 0 %<= d >%
              rotation 0 0 1 0
              children [
                Pose {
                  rotation 1 0 0 %<= alpha >%
                  children IS nextJoints
                }
              ]
            }
          ]
        }
      ]
      boundingObject USE GEOMETRY%<=name>%
      physics Physics {
      }
    }
  }
}