## Step 1: Define the building blocks

Link is Physics3D.Bodies.RigidBody:
    scale is Real: 1.0
    geometry is Physics3D.Geometries.ExternalTriMeshGeometry:
        path: $"{AGX_DATA_DIR}/models/openplx/link_with_holes.obj"
        scale: {scale, scale, scale}
    visual is Visuals.Geometries.ExternalTriMeshGeometry:
        path: geometry.path
        scale: geometry.scale

HoleConnector is Physics3D.Interactions.MateConnector:
    scale is Real: 1.0
    local_position is Real
    main_axis: Math.Vec3.Z_AXIS()
    position: Math.Vec3.from_xyz(0.013 * scale + local_position,0.014 * scale, -0.00225 * scale)

LinkWithHoles is Link:
    hole_separation is Real: 0.032 * scale
    hole_A is HoleConnector:
        scale: scale
        local_position: hole_separation * 0
    hole_B is HoleConnector:
        scale: scale
        local_position: hole_separation * 1
    hole_C is HoleConnector:
        scale: scale
        local_position: hole_separation * 2
    hole_D is HoleConnector:
        scale: scale
        local_position: hole_separation * 3
    hole_E is HoleConnector:
        scale: scale
        local_position: hole_separation * 4
    hole_F is HoleConnector:
        scale: scale
        local_position: hole_separation * 5


TwoHingedLinks is Physics3D.System:
    link_1 is LinkWithHoles
    link_2 is LinkWithHoles
    link_hinge is Physics3D.Interactions.Hinge:
        connectors: [link_1.hole_A, link_2.hole_F]

TwoNonCollidingHingedLinks is TwoHingedLinks:
    link_1.hole_A.position.z: 0.0005
    link_2.hole_F.position.z: -0.0045 - 0.0005

TwoNonCollidingHingedLinksWorld is TwoNonCollidingHingedLinks:
    world_connector is Physics3D.Interactions.MateConnector:
        main_axis: Math.Vec3.Y_AXIS()
    world_hinge is Physics3D.Interactions.Hinge:
        connectors: [link_1.hole_F, world_connector]


CylindricalDoublePendulum is Physics3D.System:
    world_connector is Physics3D.Interactions.MateConnector:
        main_axis: Math.Vec3.Y_AXIS()
    world_hinge is Physics3D.Interactions.Hinge:
        connectors: [world_connector, link_1.hole_F]
        initial_angle: Math.PI*0.5
    link_1 is LinkWithHoles:
        hole_A.position.z: 0.0005
    link_2 is LinkWithHoles:
        hole_F.position.z: -0.0045 - 0.0005
    link_cylindrical is Physics3D.Interactions.Cylindrical:
        initial_position: 0.1
        connectors: [link_1.hole_A, link_2.hole_F]
        flexibility becomes Physics3D.Interactions.Flexibility.LinearElasticCylindricalFlexibility:
            around_normal.stiffness: 0.1
            around_cross.stiffness: 0.1
    linear_spring is Physics3D.Interactions.LinearSpring:
        connectors: link_cylindrical.connectors
        position: link_cylindrical.initial_position

CylindricalDoublePendulumSwappedOrder is CylindricalDoublePendulum:
    link_cylindrical.connectors: [link_2.hole_F, link_1.hole_A]

AxleNoGeometry is Physics3D.Bodies.RigidBody:
    inertia.mass: 0.01
    inertia.tensor: Physics3D.Bodies.Inertia.symmetric_tensor({0.001,0.01,0.001}, 0, 0, 0)
    visual is Visuals.Geometries.Cylinder:
        radius: 0.0025
        height: 0.3
    connector is Physics3D.Interactions.MateConnector:
        main_axis: Math.Vec3.Y_AXIS()

TwoLinksOneAxle is Physics3D.System:
    link_1 is LinkWithHoles
    link_2 is LinkWithHoles
    axle is AxleNoGeometry:
        is_dynamic: false
    link_1_mate is Physics3D.Interactions.Cylindrical:
        connectors: [link_1.hole_B, axle.connector]
        initial_position: 0.05
        initial_angle: 0
    link_2_mate is Physics3D.Interactions.Cylindrical:
        connectors: [link_2.hole_D, axle.connector]
        initial_position: -0.05
        initial_angle: 0

TwoLinksTwoAxles is TwoLinksOneAxle:
    axle_2 is AxleNoGeometry
    link_1_mate_2 is Physics3D.Interactions.Cylindrical:
        connectors: [link_1.hole_D, axle_2.connector]
        initial_position: 0.05
    link_2_mate_2 is Physics3D.Interactions.Cylindrical:
        connectors: [link_2.hole_F, axle_2.connector]
        initial_position: -0.05

CollidingAxle is AxleNoGeometry:
    geometry is Physics3D.Geometries.Cylinder:
        radius: visual.radius
        height: visual.height

TwoLinksTwoAxlesCollision is TwoLinksTwoAxles:
    axle becomes CollidingAxle
    axle_2 becomes CollidingAxle
    link_1_mate.enabled: false
    link_2_mate.enabled: false
    link_1_mate_2.enabled: false
    link_2_mate_2.enabled: false
