Means - Cargo crane

CargoCraneComponent.zig

// MIT License
// 
// Copyright (c) 2018-2023 Madrigal Ltd.
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

const std = @import("std");
const basis = @import("basis");
const means = @import("../means.zig");

const AvatarTrackingComponent = basis.component_contexts.AvatarTrackingComponent;

const Vec2 = basis.math.Vec2;
const Vec3 = basis.math.Vec3;
const SceneNodePtr = basis.math.SceneNodePtr;
const Quaternion = basis.math.Quaternion;
const Mat43 = basis.math.Mat43;

const Message = basis.messaging.Message;
const MessageParameters = basis.messaging.MessageParameters;

const TwoJointCraneArm = means.two_joint_crane_arm.TwoJointCraneArm;
const CraneCableMagnetAssembly = means.crane_cable_magnet_assembly.CraneCableMagnetAssembly;

const RenderObject = means.render_object.RenderObject;

pub const CargoCraneComponent = struct {
    const Self = @This();
    pub const RegistrationName = "means.CargoCraneComponent";
    pub const UpdateOrder = 50;

    //----------------------------------------------------

    context: AvatarTrackingComponent,

    arm: TwoJointCraneArm,
    cableMagnetAssembly: CraneCableMagnetAssembly,

    testBox: RenderObject = .{},

    //----------------------------------------------------

    pub fn init(context: AvatarTrackingComponent) !Self {
        return Self{
            .context = context,
            .arm = TwoJointCraneArm.init(context),
            .cableMagnetAssembly = try CraneCableMagnetAssembly.init(context),
        };
    }

    //----------------------------------------------------

    // pub fn create(self: *Self) !void {
    //     _ = self;
    // }

    pub fn destroy(self: *Self) !void {
        self.cableMagnetAssembly.destroy();
        self.arm.destroy();

        self.testBox.deinit();
    }

    pub fn onObjectCreated(self: *Self) !void {
        // TODO: Some of these should probably be exposed through BPPs.
        const armParams = TwoJointCraneArm.Params{
            // This is where, in the local space of the parent object, to put the crane.
            .armRootPosition = Vec3.init(3.0, 4.0, -10.0),
            // This is essentially where, the local space of the root mesh, the joint between the root and bone0 sits.
            .armBone0JointOffset = Vec3.init(0.0, 1.351, -0.2773),
            .armBone0Length = 5.0,
            .armBone1Length = 10.0,
            .restPoseRotation = 0.0,
            //.restPoseRotation = -std.math.pi / 2.0, // Use (-std.math.pi / 2.0) to have it face to the right.
            .restPoseVerticalOffset = 2.0,
            .restPoseHorizontalOffset = 6.5,
            .craneRotationSpeed = 0.003,
            .upDownSpeed = 0.01,
            .forwardBackwardSpeed = 0.01,
        };

        self.arm.create(armParams);

        const asmParams = CraneCableMagnetAssembly.Params{
            .armEndInitialPosLocal = self.arm.getArmEndPos(),
            .cableInitialLength = 1.0,
            .cableMinLength = 1.0,
            .cableMaxLength = 30.0,
            .cableAdjustLengthSpeed = 0.02,
            .magnetMeshResourcePath = "means/vehicles/cargo_crane/magnet_blockout.binmesh",
            .magnetMaterialResourcePath = "means/vehicles/cargo_crane/magnet_blockout.binmaterial",
        };

        try self.cableMagnetAssembly.create(asmParams);
    }

    pub fn update(self: *Self, deltaTime: f32) !void {
        self.arm.update(deltaTime);

        const armEndPosWorld = self.arm.getArmEndPos();

        try self.cableMagnetAssembly.update(deltaTime, armEndPosWorld);
    }

    pub fn preTick(self: *Self, tickDeltaTime: f32) !void {
        if (self.context.onServer()) {
            // Pre-tick and apply inputs to the arm.

            var leftRight: f32 = 0.0;
            var upDown: f32 = 0.0;
            var forwardBackward: f32 = 0.0;

            if (self.context.getAvatarHostID() != -1) {
                if (self.context.getInputState(means.InputID.ProtoCraneLeft)) {
                    leftRight -= 1.0;
                }

                if (self.context.getInputState(means.InputID.ProtoCraneRight)) {
                    leftRight += 1.0;
                }

                if (self.context.getInputState(means.InputID.ProtoCraneForward)) {
                    forwardBackward += 1.0;
                }

                if (self.context.getInputState(means.InputID.ProtoCraneBackward)) {
                    forwardBackward -= 1.0;
                }

                if (self.context.getInputState(means.InputID.ProtoCraneUp)) {
                    upDown += 1.0;
                }

                if (self.context.getInputState(means.InputID.ProtoCraneDown)) {
                    upDown -= 1.0;
                }
            }

            self.arm.preTick(
                tickDeltaTime,
                leftRight,
                upDown,
                forwardBackward,
            );

            // Pre-tick and apply inputs to the cable-magnet-assembly.

            var toggleMagnetInput: bool = false;
            var cableLengthAdjustInput: f32 = 0.0;

            const worldMatrix = self.context.transform.getWorldMatrix();
            const invWorldMatrix = worldMatrix.inverse();

            const armEndPosWorld = self.arm.getArmEndPos();
            const armEndPosLocal = invWorldMatrix.transformPoint(armEndPosWorld);

            if (self.context.getAvatarHostID() != -1) {
                toggleMagnetInput = self.context.getInputAction(means.InputID.ProtoCraneMagnetToggle);

                if (self.context.getInputState(means.InputID.ProtoCraneExtendCable)) {
                    cableLengthAdjustInput += 1.0;
                }

                if (self.context.getInputState(means.InputID.ProtoCraneRetractCable)) {
                    cableLengthAdjustInput -= 1.0;
                }
            }

            try self.cableMagnetAssembly.preTick(
                armEndPosLocal,
                toggleMagnetInput,
                cableLengthAdjustInput,
            );
        }
    }

    pub fn tick(self: *Self, tickDeltaTime: f32) !void {
        self.arm.tick(tickDeltaTime);
        try self.cableMagnetAssembly.tick(tickDeltaTime);
    }

    // pub fn onMessageReceived(self: *Self, message: Message, senderNameHash: basis.string.StringHash, parameters: *MessageParameters) !void {
    //     _ = self;
    //     _ = message;
    //     _ = senderNameHash;
    //     _ = parameters;
    // }

    pub fn onPipeDataReceived(self: *Self, pipe: basis.network.PipeID, data: []const u8) !void {
        try self.cableMagnetAssembly.onPipeDataReceived(pipe, data);
    }

    pub fn onBecameClientLocalAvatar(self: *Self) !void {
        _ = self;
    }

    pub fn onLostClientLocalAvatar(self: *Self) !void {
        _ = self;
    }

    pub fn onBecameServerAvatar(self: *Self, hostID: i32) !void {
        _ = self;
        _ = hostID;
    }

    pub fn onLostServerAvatar(self: *Self, hostID: i32) !void {
        _ = self;
        _ = hostID;
    }
};

crane_cable_magnet_assembly.zig

// MIT License
// 
// Copyright (c) 2018-2023 Madrigal Ltd.
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

const std = @import("std");
const basis = @import("basis");
const means = @import("../means.zig");

const AvatarTrackingComponent = basis.component_contexts.AvatarTrackingComponent;

const Vec2 = basis.math.Vec2;
const Vec3 = basis.math.Vec3;
const SceneNodePtr = basis.math.SceneNodePtr;
const Quaternion = basis.math.Quaternion;
const Mat43 = basis.math.Mat43;

const PhysicsScenePtr = basis.physics.PhysicsScenePtr;
const PhysicsShapePtr = basis.physics.PhysicsShapePtr;
const PhysicsActorPtr = basis.physics.PhysicsActorPtr;
const PhysicsJointPtr = basis.physics.PhysicsJointPtr;
const PhysicsEnginePtr = basis.physics.PhysicsEnginePtr;
const PhysicsTransform = basis.physics.PhysicsTransform;

const GameObjectPtr = basis.game_object.GameObjectPtr;

const RenderObject = means.render_object.RenderObject;

//----------------------------------------------------

const FIXTURE_MASS = 10.0;
const FIXTURE_JOINT_STIFFNESS = 4000000.0;
const FIXTURE_JOINT_DAMPING = 300.0;
const FIXTURE_JOINT_FORCE_LIMIT = 100000.0;

const JOINT_PROJECTION_LIN_TOLERANCE = 0.25;
const JOINT_PROJECTION_ANG_TOLERANCE = 0.5;

const CABLE_JOINT_STIFFNESS = 200.0;
const CABLE_JOINT_DAMPING = 1000.0;
const CABLE_JOINT_FORCE_LIMIT = 100000000.0;

const CABLE_SEGMENT_HEIGHT = 0.5;
const CABLE_SEGMENT_RADIUS = 0.08;
const CABLE_SEGMENT_MASS = 30.0;

const MAGNET_THICKNESS = 0.3;
const MAGNET_RADIUS = 0.8;
const MAGNET_MASS = 200.0;
// Moving the CoM down a bit makes the magnet stay upright more easily.
const MAGNET_CENTER_OF_MASS = Vec3.init(0.0, -0.5, 0.0);

fn getFixtureVerticalOffset(cableTotalLength: f32) f32 {
    // Note the minus sign. The offset is given as distance below the
    // arm end position, in local space. So it never goes above zero.
    return -@mod(
        cableTotalLength,
        CABLE_SEGMENT_HEIGHT,
    );
}

//----------------------------------------------------

pub const CraneCableMagnetAssembly = struct {
    const Self = @This();

    const CABLE_VISUALIZATION_BUFFER_SIZE = 1 * 1024;

    pub const Params = struct {
        armEndInitialPosLocal: Vec3 = Vec3.Zero,
        cableInitialLength: f32 = 1.0,
        cableMinLength: f32 = 1.0,
        cableMaxLength: f32 = 30.0,
        cableAdjustLengthSpeed: f32 = 0.01,

        magnetMeshResourcePath: []const u8 = "",
        magnetMaterialResourcePath: []const u8 = "",
    };

    //----------------------------------------------------

    context: AvatarTrackingComponent,

    physicsEngine: PhysicsEnginePtr = PhysicsEnginePtr.initNull(),
    physicsScene: PhysicsScenePtr = PhysicsScenePtr.initNull(),

    params: Params = .{},

    fixture: PhysicsPart = .{},
    cableSegments: std.ArrayList(PhysicsPart),
    magnet: PhysicsPart = .{},

    hasTickedPhysics: bool = false,
    currentCableLength: f32 = 0.0,

    targetJoint: PhysicsJointPtr = PhysicsJointPtr.initNull(),

    cablePointBounds: basis.math.AABB = basis.math.AABB.initEmpty(),
    cableVisualizationBuffer: []u8,
    cableVisualizationPipe: basis.network.PipeID = 0,
    cablePackedVisualizationPoints: std.ArrayList(u32),

    cableUnpackedPositions: std.ArrayList(Vec3),

    magnetRenderObject: RenderObject = .{},
    cableRenderObject: means.crane_cable_render_object.CraneCableRenderObject = .{},

    //----------------------------------------------------

    pub fn init(
        context: AvatarTrackingComponent,
    ) !Self {
        return Self{
            .context = context,
            .cableSegments = std.ArrayList(PhysicsPart).init(context.allocator),
            .cableVisualizationBuffer = try context.allocator.alloc(u8, CABLE_VISUALIZATION_BUFFER_SIZE),
            .cablePackedVisualizationPoints = std.ArrayList(u32).init(context.allocator),
            .cableUnpackedPositions = std.ArrayList(Vec3).init(context.allocator),
        };
    }

    pub fn create(self: *Self, params: Params) !void {
        self.params = params;

        self.currentCableLength = params.cableInitialLength;

        self.physicsEngine = self.context.getPhysicsEngine();
        self.physicsScene = self.context.getPrimaryPhysicsScene();

        self.cableVisualizationPipe = self.context.registerPipe(
            "cableVisualizationPipe",
            .ServerToClient,
            false,
        );

        if (self.context.onServer()) {
            try self.createPhysicsParts();
        } else {
            self.createVisuals();
        }
    }

    pub fn destroy(self: *Self) void {
        if (self.context.onServer()) {
            self.destroyPhysicsParts();
        } else {
            self.destroyVisuals();
        }

        self.cableSegments.deinit();

        self.context.allocator.free(self.cableVisualizationBuffer);

        self.cablePackedVisualizationPoints.deinit();

        self.cableUnpackedPositions.deinit();
    }

    pub fn update(self: *Self, deltaTime: f32, armEndPosition: Vec3) !void {
        _ = deltaTime;

        if (self.context.onClient()) {
            const interpolatedPos = self.context.transform.getRenderSceneNode().getPositionInSpace(.World);
            try self.unpackCablePoints(interpolatedPos, armEndPosition);

            try self.cableRenderObject.update(self.cableUnpackedPositions.items);
        }
    }

    pub fn preTick(
        self: *Self,
        armEndGoalPosLocal: Vec3, // The position (in the local space of the parent) of the arm end.
        toggleMagnetInput: bool,
        cableLengthAdjustInput: f32,
    ) !void {
        if (self.context.onServer()) {
            // NB! The order matters here: First Extend/retract and then set the fixture joint goal.
            // Doing these things in reverse order introduces noticable choppiness when adjusting the
            // cable length.

            // Extend/retract the cable.
            if (!basis.math.floatsAlmostEqual(cableLengthAdjustInput, 0.0)) {
                if (cableLengthAdjustInput > 0.0) {
                    try self.extendCable(armEndGoalPosLocal);
                } else {
                    self.retractCable();
                }
            }

            // Update the transform drive goal of the fixture's Dof6 joint.
            if (self.hasTickedPhysics) {
                // If we start driving the fixture joint immediately after adding it to the scene
                // we can some weird behavior where the whole object rocks and the fixture is seen
                // approaching the object at a high velocity (in PvD). A workaround seems to be to
                // only start driving the joint after the physics have ticked at least once.

                // The goal is "relative to the constraint frame of actor[0]" so we subtract the inital pos
                // to get the offset the joint expects.
                var armEndGoalOffset = armEndGoalPosLocal.sub(self.params.armEndInitialPosLocal);
                armEndGoalOffset.y += getFixtureVerticalOffset(self.currentCableLength);
                const armEndGoalPose = PhysicsTransform.init(armEndGoalOffset, Quaternion.Identity);
                self.fixture.joint.setDriveGoalPose(armEndGoalPose);
            }

            if (toggleMagnetInput) {
                self.toggleMagnet();
            }
        }
    }

    pub fn tick(self: *Self, tickDeltaTime: f32) !void {
        _ = tickDeltaTime;

        self.hasTickedPhysics = true;

        if (self.context.onServer()) {
            const pos = self.context.transform.getPosition();
            try self.packCablePoints(pos);

            var stream = basis.binary_stream.BinaryWriteStream.init(
                self.cableVisualizationBuffer,
                true,
            );

            stream.put(basis.math.AABB, self.cablePointBounds);

            stream.putInt(u32, @intCast(self.cablePackedVisualizationPoints.items.len));
            for (self.cablePackedVisualizationPoints.items) |p| {
                stream.putInt(u32, p);
            }

            {
                // We want the magnet transform in the local space of the vehicle but the
                // physics engine gives it in world space, so we have to transform it
                // before writing to the buffer.

                const magnetTransform = self.magnet.rigidBody.getWorldTransform();

                const worldToLocalMatrix = self.context.transform.getWorldMatrix().inverse();
                const worldToLocalRot = Quaternion.initFromRotationMatrix(worldToLocalMatrix);

                const localMagnetPos = worldToLocalMatrix.transformPoint(magnetTransform.position);
                const localMagnetOri = magnetTransform.orientation.concatenate(worldToLocalRot);

                stream.put(Vec3, localMagnetPos);
                stream.put(Quaternion, localMagnetOri);
            }

            const data = self.cableVisualizationBuffer[0..stream.cursorPosition];
            self.context.writeToPipe(self.cableVisualizationPipe, data);
        }
    }

    // This needs to be called by the owning component.
    pub fn onPipeDataReceived(self: *Self, pipe: basis.network.PipeID, data: []const u8) !void {
        basis.assert(pipe == self.cableVisualizationPipe);
        basis.assert(self.context.onClient());

        var stream = basis.binary_stream.BinaryReadStream.init(data, true);

        self.cablePointBounds = stream.get(basis.math.AABB);

        const pointCount = stream.getInt(u32);

        self.cablePackedVisualizationPoints.clearRetainingCapacity();
        try self.cablePackedVisualizationPoints.ensureTotalCapacity(pointCount);

        {
            var i: usize = 0;
            while (i < pointCount) : (i += 1) {
                const point = stream.getInt(u32);
                self.cablePackedVisualizationPoints.appendAssumeCapacity(point);
            }
        }

        if (!self.magnetRenderObject.meshInstance.isNull()) {
            const magnetPos = stream.get(Vec3);
            const magnetOri = stream.get(Quaternion);

            self.magnetRenderObject.sceneNode.setPosition(magnetPos);
            self.magnetRenderObject.sceneNode.setOrientation(magnetOri);
        }
    }

    //----------------------------------------------------

    fn createPhysicsParts(self: *Self) !void {
        const parentGameObject = self.context.getGameObject();
        const parentPhysicsActor = self.context.transform.getPhysicsActor();
        const worldMatrix = self.context.transform.getWorldMatrix();

        const segmentCount: usize = @intFromFloat(
            @divFloor(self.currentCableLength, CABLE_SEGMENT_HEIGHT),
        );

        //----------------------------------------------------
        // Fixture
        //----------------------------------------------------

        self.fixture = PhysicsPart.createFixture(
            self.physicsEngine,
            self.physicsScene,
            parentGameObject,
            parentPhysicsActor,
            self.params,
            worldMatrix,
        );

        //----------------------------------------------------
        // Cable
        //----------------------------------------------------

        const cableFixturePos = worldMatrix.transformPoint(self.params.armEndInitialPosLocal);
        var bottomOfCurrentSegment: Vec3 = cableFixturePos;

        const localUpDir = worldMatrix.getY();
        const localDownDir = localUpDir.negated();

        try self.cableSegments.ensureTotalCapacity(segmentCount);

        var i: usize = 0;
        while (i < segmentCount) : (i += 1) {
            bottomOfCurrentSegment = bottomOfCurrentSegment.add(localDownDir.multiplyFloat(CABLE_SEGMENT_HEIGHT));
            const centerOfCurrentSegment = bottomOfCurrentSegment.sub(localDownDir.multiplyFloat(CABLE_SEGMENT_HEIGHT * 0.5));

            var segment = PhysicsPart.createCableSegmentWithoutJoint(
                self.physicsEngine,
                self.physicsScene,
                parentGameObject,
                centerOfCurrentSegment,
                null,
                worldMatrix,
            );

            // Create the joint. If i == 0 we connect the segment to the fixture.
            // Otherwise, we connect it to the previous segment.
            {
                const actorA = if (i == 0)
                    self.fixture.rigidBody
                else blk: {
                    const prevSegment = self.cableSegments.getLast();
                    break :blk prevSegment.rigidBody;
                };

                const transformA = if (i == 0)
                    PhysicsTransform.Identity
                else
                    PhysicsTransform.init(
                        Vec3.init(0, -CABLE_SEGMENT_HEIGHT * 0.5, 0),
                        Quaternion.Identity,
                    );

                const actorB = segment.rigidBody;

                const transformB = PhysicsTransform.init(
                    Vec3.init(0, CABLE_SEGMENT_HEIGHT * 0.5, 0),
                    Quaternion.Identity,
                );

                segment.joint = self.physicsEngine.createSphericalSpringJoint(
                    actorA,
                    transformA,
                    actorB,
                    transformB,
                    CABLE_JOINT_STIFFNESS,
                    CABLE_JOINT_DAMPING,
                    CABLE_JOINT_FORCE_LIMIT,
                );
            }

            segment.joint.enableProjection(
                true,
                JOINT_PROJECTION_LIN_TOLERANCE,
                JOINT_PROJECTION_ANG_TOLERANCE,
            );
            self.physicsScene.addJoint(segment.joint);

            try self.cableSegments.append(segment);
        }

        //----------------------------------------------------
        // Magnet
        //----------------------------------------------------

        const lastCableSegment = self.cableSegments.getLast();
        const centerOfMagnet = bottomOfCurrentSegment.add(localDownDir.multiplyFloat(MAGNET_THICKNESS * 0.5));

        self.magnet = PhysicsPart.createMagnet(
            self.physicsEngine,
            self.physicsScene,
            parentGameObject,
            lastCableSegment,
            centerOfMagnet,
            worldMatrix,
        );

        // Reset the has-ticked flag.
        self.hasTickedPhysics = false;
    }

    fn destroyPhysicsParts(self: *Self) void {
        if (!self.targetJoint.isNull()) {
            self.physicsScene.removeJoint(self.targetJoint);
            self.targetJoint.releaseAndZero();
        }

        self.magnet.removeAndDestroy(self.physicsScene);

        for (self.cableSegments.items) |*segment| {
            segment.removeAndDestroy(self.physicsScene);
        }
        self.cableSegments.clearRetainingCapacity();

        self.fixture.removeAndDestroy(self.physicsScene);

        self.hasTickedPhysics = false;
    }

    //----------------------------------------------------

    fn createVisuals(self: *Self) void {
        const renderer = self.context.getRenderer();
        const parentNode = self.context.transform.getRenderSceneNode();
        const go = self.context.getGameObject();

        self.magnetRenderObject = RenderObject.init(
            renderer,
            true,
            RenderObject.MeshSource{ .meshResourcePath = self.params.magnetMeshResourcePath },
            self.params.magnetMaterialResourcePath,
            parentNode,
            true,
            go,
        );

        const cableParams = means.crane_cable_render_object.CraneCableRenderObject.Params{
            .materialResourcePath = "means/vehicles/cargo_crane/cable.binmaterial",
            .cableThickness = 0.03,
            .maxCablePointCount = 128,
            .smoothUsingCurvePath = true,
        };

        self.cableRenderObject = means.crane_cable_render_object.CraneCableRenderObject.init(
            cableParams,
            renderer,
            go,
            self.context.allocator,
        );
    }

    fn destroyVisuals(self: *Self) void {
        self.cableRenderObject.deinit();
        self.magnetRenderObject.deinit();
    }

    //----------------------------------------------------

    fn extendCable(
        self: *Self,
        armEndGoalPosLocal: Vec3,
    ) !void {
        if (self.currentCableLength >= self.params.cableMaxLength) return;

        const prevVerticalOffset = getFixtureVerticalOffset(self.currentCableLength);

        self.currentCableLength += self.params.cableAdjustLengthSpeed;
        self.currentCableLength = @min(self.currentCableLength, self.params.cableMaxLength);
        //basis.printf("Cable length: {d:.2}\n", .{self.currentCableLength});

        const newVerticalOffset = getFixtureVerticalOffset(self.currentCableLength);

        if (newVerticalOffset > prevVerticalOffset) {
            // The new offset is larger. We have jumped back up and need to add a new segment to the cable.
            // "Segment A" is the previous "first segment" of the cable. And "Segment B" is the new segment
            // which will be added below.

            const parentGameObject = self.context.getGameObject();
            const worldMatrix = self.context.transform.getWorldMatrix();

            var segmentA: *PhysicsPart = &self.cableSegments.items[0];

            // Disconnect SegmentA from the fixture.
            self.physicsScene.removeJoint(segmentA.joint);
            segmentA.joint.release();

            var fixtureNewPosition = armEndGoalPosLocal;
            fixtureNewPosition.y += getFixtureVerticalOffset(self.currentCableLength);
            fixtureNewPosition = worldMatrix.transformPoint(fixtureNewPosition);

            const fixtureTransform = self.fixture.rigidBody.getWorldTransform();
            const segmentATransform = segmentA.rigidBody.getWorldTransform();

            const centerOfNewSegment = fixtureNewPosition.add(segmentATransform.position).multiplyFloat(0.5);
            const newSegmentRot = Quaternion.slerp(0.5, fixtureTransform.orientation, segmentATransform.orientation);

            var segmentB = PhysicsPart.createCableSegmentWithoutJoint(
                self.physicsEngine,
                self.physicsScene,
                parentGameObject,
                centerOfNewSegment,
                newSegmentRot,
                worldMatrix,
            );

            {
                // Add a new joint to segment A, between B and A.

                const actorA = segmentB.rigidBody;

                const transformA = PhysicsTransform.init(
                    Vec3.init(0, -CABLE_SEGMENT_HEIGHT * 0.5, 0),
                    Quaternion.Identity,
                );

                const actorB = segmentA.rigidBody;

                const transformB = PhysicsTransform.init(
                    Vec3.init(0, CABLE_SEGMENT_HEIGHT * 0.5, 0),
                    Quaternion.Identity,
                );

                segmentA.joint = self.physicsEngine.createSphericalSpringJoint(
                    actorA,
                    transformA,
                    actorB,
                    transformB,
                    CABLE_JOINT_STIFFNESS,
                    CABLE_JOINT_DAMPING,
                    CABLE_JOINT_FORCE_LIMIT,
                );

                segmentA.joint.enableProjection(
                    true,
                    JOINT_PROJECTION_LIN_TOLERANCE,
                    JOINT_PROJECTION_ANG_TOLERANCE,
                );

                self.physicsScene.addJoint(segmentA.joint);
            }

            {
                // Add a new joint to segment B, between the fixture and B.

                const actorA = self.fixture.rigidBody;

                const transformA = PhysicsTransform.Identity;

                const actorB = segmentB.rigidBody;

                const transformB = PhysicsTransform.init(
                    Vec3.init(0, CABLE_SEGMENT_HEIGHT * 0.5, 0),
                    Quaternion.Identity,
                );

                segmentB.joint = self.physicsEngine.createSphericalSpringJoint(
                    actorA,
                    transformA,
                    actorB,
                    transformB,
                    CABLE_JOINT_STIFFNESS,
                    CABLE_JOINT_DAMPING,
                    CABLE_JOINT_FORCE_LIMIT,
                );

                segmentB.joint.enableProjection(
                    true,
                    JOINT_PROJECTION_LIN_TOLERANCE,
                    JOINT_PROJECTION_ANG_TOLERANCE,
                );

                self.physicsScene.addJoint(segmentB.joint);
            }

            try self.cableSegments.insert(0, segmentB);
        }
    }

    fn retractCable(self: *Self) void {
        if (self.currentCableLength <= self.params.cableMinLength) return;

        const prevVerticalOffset = getFixtureVerticalOffset(self.currentCableLength);

        self.currentCableLength -= self.params.cableAdjustLengthSpeed;
        self.currentCableLength = @max(self.currentCableLength, self.params.cableMinLength);
        //basis.printf("Cable length: {d:.2}\n", .{self.currentCableLength});

        const newVerticalOffset = getFixtureVerticalOffset(self.currentCableLength);

        if (newVerticalOffset < prevVerticalOffset) {
            // The new offset is smaller. We have jumped back down and need to remove a segment from the cable.
            // "Segment A" is the "first segment" of the cable, ie. the segment which connects to the fixture.
            // "Segment B" is the next segment after Segment A, ie. the one which will become the new first segment.

            basis.assert(self.cableSegments.items.len > 1);

            var segmentA: *PhysicsPart = &self.cableSegments.items[0];
            var segmentB: *PhysicsPart = &self.cableSegments.items[1];

            // Release/remove/destroy segment A's stuff.
            segmentA.removeAndDestroy(self.physicsScene);

            // Remove the joint between segment B and A.
            self.physicsScene.removeJoint(segmentB.joint);
            segmentB.joint.release();

            {
                // Add a new joint to segment B, between the fixture and segment B.

                const actorA = self.fixture.rigidBody;

                const transformA = PhysicsTransform.Identity;

                const actorB = segmentB.rigidBody;

                const transformB = PhysicsTransform.init(
                    Vec3.init(0, CABLE_SEGMENT_HEIGHT * 0.5, 0),
                    Quaternion.Identity,
                );

                segmentB.joint = self.physicsEngine.createSphericalSpringJoint(
                    actorA,
                    transformA,
                    actorB,
                    transformB,
                    CABLE_JOINT_STIFFNESS,
                    CABLE_JOINT_DAMPING,
                    CABLE_JOINT_FORCE_LIMIT,
                );

                segmentB.joint.enableProjection(
                    true,
                    JOINT_PROJECTION_LIN_TOLERANCE,
                    JOINT_PROJECTION_ANG_TOLERANCE,
                );

                self.physicsScene.addJoint(segmentB.joint);
            }

            // Remove segment A from the list.
            _ = self.cableSegments.orderedRemove(0);
        }
    }

    fn toggleMagnet(self: *Self) void {
        if (!self.targetJoint.isNull()) {
            self.physicsScene.removeJoint(self.targetJoint);
            self.targetJoint.releaseAndZero();

            basis.printf("Magnet: off\n", .{});
        } else {
            const magnetTransform = self.magnet.rigidBody.getWorldTransform();
            const magnetLocalToWorldTransform = Mat43.fromOrientationPosition(magnetTransform.orientation, magnetTransform.position);
            const magnetWorldToLocalTransform = magnetLocalToWorldTransform.inverse();

            const blockingActors: u32 = basis.physics.physics_actor.PhysicsActorType.RigidBodyDynamic.asUint();

            const MAX_HIT_COUNT = 16;
            var hitResults: [MAX_HIT_COUNT]basis.physics.RayCastResult = undefined;

            const RAY_LENGTH = 0.5;
            const SPHERE_RADIUS = 0.1;

            const rayStart = magnetTransform.position;
            const rayEnd = Vec3.init(0.0, -RAY_LENGTH, 0.0).transform(magnetLocalToWorldTransform);
            const rayDir = (rayEnd.sub(rayStart)).normalized();

            //var result: basis.physics.RayCastResult = basis.physics.RayCastResult.initZero();

            const hitCount = self.physicsScene.sphereSweepEx(SPHERE_RADIUS, rayStart, rayDir, RAY_LENGTH, &hitResults, blockingActors);

            if (hitCount > 0) {
                const targetIndex = self.findTargetIndex(hitResults[0..hitCount]);

                if (targetIndex == null) {
                    basis.printf("Magnet: (no target in range)\n", .{});
                    return;
                }

                const target = hitResults[targetIndex.?].getPhysicsActor();
                const targetTransform = target.getWorldTransform();
                const targetLocalToWorldTransform = Mat43.fromOrientationPosition(targetTransform.orientation, targetTransform.position);
                const targetWorldToLocalTransform = targetLocalToWorldTransform.inverse();

                // We specify the hit point in each of the objects' local spaces to fix them
                // to each other. Using the inverses of the objects' orientations makes the
                // joint take the objects' current relative rotations into account, fixing
                // them without rotating them further.

                const magnetFixPoint = hitResults[targetIndex.?].hitPoint.transform(magnetWorldToLocalTransform);
                const targetFixPoint = hitResults[targetIndex.?].hitPoint.transform(targetWorldToLocalTransform);

                const transformA = PhysicsTransform.init(magnetFixPoint, magnetTransform.orientation.inverse());
                const transformB = PhysicsTransform.init(targetFixPoint, targetTransform.orientation.inverse());

                self.targetJoint = self.physicsEngine.createFixedJoint(self.magnet.rigidBody, transformA, target, transformB);

                // Might be best to not project the target object, in case it has dynamic objects on top of it...
                //self.targetJoint.enableProjection(true, 0.25, 0.5);

                self.physicsScene.addJoint(self.targetJoint);

                basis.printf("Magnet: on\n", .{});
            } else {
                basis.printf("Magnet: (no target in range)\n", .{});
            }
        }
    }

    // Given a list of ray cast results, returns the index of the first object in the list
    // which is not a hit against the crane's own rigid bodies, or null if no such hit exists.
    fn findTargetIndex(self: *const Self, hitResults: []basis.physics.RayCastResult) ?usize {
        for (hitResults, 0..) |hitResult, i| {
            const actor = hitResult.getPhysicsActor();
            const actorCppPtr = actor.cppPtr;
            if (actorCppPtr == 0) continue;

            if (actorCppPtr == self.fixture.rigidBody.cppPtr) continue;
            if (actorCppPtr == self.magnet.rigidBody.cppPtr) continue;

            var partOfCable = false;
            for (self.cableSegments.items) |s| {
                if (s.rigidBody.cppPtr == actorCppPtr) {
                    partOfCable = true;
                    break;
                }
            }

            if (partOfCable) continue;

            return i;
        }

        return null;
    }

    fn packCablePoints(self: *Self, parentPosition: Vec3) !void {
        // We subtract the parent position, ie. the vehicle position, from
        // the world-space position, to get the points in the "unrotated
        // local space" of the vehicle.

        self.cablePointBounds.clear();
        for (self.cableSegments.items) |s| {
            const pos = s.rigidBody.getWorldTransform().position.sub(parentPosition);
            self.cablePointBounds.addPoint(pos);
        }

        self.cablePackedVisualizationPoints.clearRetainingCapacity();

        const boundsMin = self.cablePointBounds.min;
        const boundsMax = self.cablePointBounds.max;

        for (self.cableSegments.items) |s| {

            // Pack each 3D point of the cable into a u32 (x: 11 bits, y: 10 bits, z: 11 bits) where each
            // point is inside the cable point bounds. The precision is just enough to allow for smooth
            // movement inside the bounds.

            const pos = s.rigidBody.getWorldTransform().position.sub(parentPosition);

            const scaledX = if (basis.math.floatsAlmostEqual(boundsMin.x, boundsMax.x))
                0.0
            else
                basis.math.remapFloat(pos.x, boundsMin.x, boundsMax.x, 0.0, 2047.0); // 11 bits, max = 2047

            const scaledY = if (basis.math.floatsAlmostEqual(boundsMin.y, boundsMax.y))
                0.0
            else
                basis.math.remapFloat(pos.y, boundsMin.y, boundsMax.y, 0.0, 1023.0); // 10 bits, max = 1023

            const scaledZ = if (basis.math.floatsAlmostEqual(boundsMin.z, boundsMax.z))
                0.0
            else
                basis.math.remapFloat(pos.z, boundsMin.z, boundsMax.z, 0.0, 2047.0); // 11 bits, max = 2047

            var memory: u32 = 0;
            const memoryPtrBytes = std.mem.asBytes(&memory);

            const packedX: u11 = @intFromFloat(scaledX);
            const packedY: u10 = @intFromFloat(scaledY);
            const packedZ: u11 = @intFromFloat(scaledZ);

            std.mem.writePackedInt(u11, memoryPtrBytes, 0, packedX, std.builtin.Endian.Little);
            std.mem.writePackedInt(u10, memoryPtrBytes, 11, packedY, std.builtin.Endian.Little);
            std.mem.writePackedInt(u11, memoryPtrBytes, 21, packedZ, std.builtin.Endian.Little);

            try self.cablePackedVisualizationPoints.append(memory);
        }
    }

    fn unpackCablePoints(self: *Self, parentPosition: Vec3, armEndPosition: Vec3) !void {
        self.cableUnpackedPositions.clearRetainingCapacity();

        const boundsMin = self.cablePointBounds.min;
        const boundsMax = self.cablePointBounds.max;

        // We push the position of the arm end as the first point, to make sure the cable
        // visually always starts at that position.
        try self.cableUnpackedPositions.append(armEndPosition);

        for (self.cablePackedVisualizationPoints.items) |p| {
            const memoryPtrBytes = std.mem.asBytes(&p);

            const packedX = std.mem.readPackedInt(u11, memoryPtrBytes, 0, std.builtin.Endian.Little);
            const packedY = std.mem.readPackedInt(u10, memoryPtrBytes, 11, std.builtin.Endian.Little);
            const packedZ = std.mem.readPackedInt(u11, memoryPtrBytes, 21, std.builtin.Endian.Little);

            const scaledX: f32 = @floatFromInt(packedX);
            const scaledY: f32 = @floatFromInt(packedY);
            const scaledZ: f32 = @floatFromInt(packedZ);

            const v = basis.math.Vec3.init(
                basis.math.remapFloat(scaledX, 0, 2047.0, boundsMin.x, boundsMax.x),
                basis.math.remapFloat(scaledY, 0, 1023.0, boundsMin.y, boundsMax.y),
                basis.math.remapFloat(scaledZ, 0, 2047.0, boundsMin.z, boundsMax.z),
            );

            try self.cableUnpackedPositions.append(v.add(parentPosition));
        }
    }
};

//----------------------------------------------------

const PhysicsPart = struct {
    const Self = @This();

    pub const PartType = enum { Fixture, CableSegment, Magnet };

    shape: PhysicsShapePtr = PhysicsShapePtr.initNull(),
    rigidBody: PhysicsActorPtr = PhysicsActorPtr.initNull(),
    joint: PhysicsJointPtr = PhysicsJointPtr.initNull(),
    partType: PartType = .Fixture,

    // Create the fixture body and joint, ie. the cylinder which sits at the
    // end of the crane arm, and which the upper end of the cable is attached to.
    pub fn createFixture(
        physicsEngine: PhysicsEnginePtr,
        physicsScene: PhysicsScenePtr,
        parentGameObject: GameObjectPtr,
        parentPhysicsActor: PhysicsActorPtr,
        params: CraneCableMagnetAssembly.Params,
        worldMatrix: Mat43,
    ) Self {

        // Create the shape.

        const defaultMaterial = physicsEngine.getDefaultMaterial();

        const shape = physicsEngine.createCylinder(
            0.3,
            0.4,
            defaultMaterial,
            basis.physics.PhysicsTransform.Identity,
            true,
        );

        // Create the RB.

        const pos = worldMatrix.transformPoint(params.armEndInitialPosLocal);
        const rot = Quaternion.initFromRotationMatrix(worldMatrix);

        const rb = physicsEngine.createRigidBodyDynamic(
            &[_]PhysicsShapePtr{shape},
            FIXTURE_MASS,
            Vec3.Zero,
            PhysicsTransform.init(pos, rot),
            false,
            false,
        );

        rb.associateWithGameObject(parentGameObject);

        physicsScene.addActor(rb);

        // Create the joint.

        const joint = physicsEngine.createDof6Joint(
            parentPhysicsActor,
            PhysicsTransform.init(params.armEndInitialPosLocal, Quaternion.Identity),
            rb,
            PhysicsTransform.init(Vec3.Zero, Quaternion.Identity),
        );

        joint.setDof6Motion(.AlongX, .Free);
        joint.setDof6Motion(.AlongY, .Free);
        joint.setDof6Motion(.AlongZ, .Free);
        joint.setDof6Drive(
            .X,
            FIXTURE_JOINT_STIFFNESS,
            FIXTURE_JOINT_DAMPING,
            FIXTURE_JOINT_FORCE_LIMIT,
            false,
        );
        joint.setDof6Drive(
            .Y,
            FIXTURE_JOINT_STIFFNESS,
            FIXTURE_JOINT_DAMPING,
            FIXTURE_JOINT_FORCE_LIMIT,
            false,
        );
        joint.setDof6Drive(
            .Z,
            FIXTURE_JOINT_STIFFNESS,
            FIXTURE_JOINT_DAMPING,
            FIXTURE_JOINT_FORCE_LIMIT,
            false,
        );

        joint.enableProjection(
            true,
            JOINT_PROJECTION_LIN_TOLERANCE,
            JOINT_PROJECTION_ANG_TOLERANCE,
        );

        physicsScene.addJoint(joint);

        return Self{
            .shape = shape,
            .rigidBody = rb,
            .joint = joint,
            .partType = .Fixture,
        };
    }

    pub fn createCableSegmentWithoutJoint(
        physicsEngine: PhysicsEnginePtr,
        physicsScene: PhysicsScenePtr,
        parentGameObject: GameObjectPtr,
        centerOfSegment: Vec3,
        segmentRotation: ?Quaternion,
        worldMatrix: Mat43,
    ) Self {
        // Create the shape.

        const defaultMaterial = physicsEngine.getDefaultMaterial();

        const shape = physicsEngine.createCapsule(
            CABLE_SEGMENT_RADIUS,
            CABLE_SEGMENT_HEIGHT,
            defaultMaterial,
            basis.physics.PhysicsTransform.Identity,
            true,
        );

        // Create the RB.

        const rot = segmentRotation orelse Quaternion.initFromRotationMatrix(worldMatrix);

        const rb = physicsEngine.createRigidBodyDynamic(
            &[_]PhysicsShapePtr{shape},
            CABLE_SEGMENT_MASS,
            Vec3.Zero,
            PhysicsTransform.init(centerOfSegment, rot),
            false,
            false,
        );

        rb.associateWithGameObject(parentGameObject);

        physicsScene.addActor(rb);

        return Self{
            .shape = shape,
            .rigidBody = rb,
            .joint = PhysicsJointPtr.initNull(),
            .partType = .CableSegment,
        };
    }

    pub fn createMagnet(
        physicsEngine: PhysicsEnginePtr,
        physicsScene: PhysicsScenePtr,
        parentGameObject: GameObjectPtr,
        lastCableSegment: PhysicsPart,
        centerOfMagnet: Vec3,
        worldMatrix: Mat43,
    ) Self {

        // Create the shape.

        const defaultMaterial = physicsEngine.getDefaultMaterial();

        const shape = physicsEngine.createCylinder(
            MAGNET_RADIUS,
            MAGNET_THICKNESS,
            defaultMaterial,
            basis.physics.PhysicsTransform.Identity,
            true,
        );

        // Create the RB.

        const rot = Quaternion.initFromRotationMatrix(worldMatrix);

        const rb = physicsEngine.createRigidBodyDynamic(
            &[_]PhysicsShapePtr{shape},
            MAGNET_MASS,
            MAGNET_CENTER_OF_MASS,
            PhysicsTransform.init(centerOfMagnet, rot),
            false,
            false,
        );

        rb.associateWithGameObject(parentGameObject);

        physicsScene.addActor(rb);

        // Create the joint.

        const transformA = PhysicsTransform.init(
            Vec3.init(0, -CABLE_SEGMENT_HEIGHT * 0.5, 0),
            Quaternion.Identity,
        );
        const transformB = PhysicsTransform.init(
            Vec3.init(0, MAGNET_THICKNESS * 0.5, 0),
            Quaternion.Identity,
        );

        const joint = physicsEngine.createFixedJoint(
            lastCableSegment.rigidBody,
            transformA,
            rb,
            transformB,
        );

        joint.enableProjection(
            true,
            JOINT_PROJECTION_LIN_TOLERANCE,
            JOINT_PROJECTION_ANG_TOLERANCE,
        );
        physicsScene.addJoint(joint);

        return Self{
            .shape = shape,
            .rigidBody = rb,
            .joint = joint,
            .partType = .Magnet,
        };
    }

    pub fn removeAndDestroy(
        self: *Self,
        physicsScene: PhysicsScenePtr,
    ) void {
        if (!self.joint.isNull()) {
            physicsScene.removeJoint(self.joint);
        }

        if (!self.rigidBody.isNull()) {
            physicsScene.removeActor(self.rigidBody);
        }

        self.joint.releaseAndZero();
        self.rigidBody.releaseAndZero();
        self.shape.releaseAndZero();
    }
};

crane_cable_render_object.zig

// MIT License
// 
// Copyright (c) 2018-2023 Madrigal Ltd.
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

const std = @import("std");
const basis = @import("basis");
const means = @import("../means.zig");

const Vec2 = basis.math.Vec2;
const Vec3 = basis.math.Vec3;
const SceneNodePtr = basis.math.SceneNodePtr;
const Quaternion = basis.math.Quaternion;
const Mat43 = basis.math.Mat43;
const CurvePath3D = basis.math.CurvePath3D;
const CurvePathCalculator3D = basis.math.CurvePathCalculator3D;

const RendererPtr = basis.renderer.RendererPtr;
const RenderScenePtr = basis.renderer.RenderScenePtr;
const MeshPtr = basis.renderer.MeshPtr;
const MaterialPtr = basis.renderer.MaterialPtr;
const MeshInstancePtr = basis.renderer.MeshInstancePtr;
const MeshGeometryPtr = basis.renderer.mesh_geometry.MeshGeometryPtr;

const RenderObject = means.render_object.RenderObject;

//const MeshResourcePtr = basis.resources.MeshResourcePtr;
const MaterialResourcePtr = basis.resources.MaterialResourcePtr;

const GameObjectPtr = basis.game_object.GameObjectPtr;

//----------------------------------------------------

pub const CraneCableRenderObject = struct {
    const Self = @This();

    const VERTEX_FORMAT_TYPE = basis.renderer.vertex_formats.VertexFormatType.PositionTangentBinormalNormalTexcoord;
    const VERTEX_TYPE = basis.renderer.vertex_formats.VertexPositionTangentBinormalNormalTexcoord;
    const VERTICES_PER_SAMPLED_POINT = 4;

    pub const Params = struct {
        materialResourcePath: []const u8 = "",
        cableThickness: f32 = 0.05,
        maxCablePointCount: u32 = 128,
        smoothUsingCurvePath: bool = true,
    };

    //----------------------------------------------------

    params: Params = .{},

    renderObject: RenderObject = .{},

    bounds: basis.math.AABB = basis.math.AABB.initEmpty(),

    path: CurvePath3D = .{},
    pathCalculator: CurvePathCalculator3D = .{},
    smoothedPoints: std.ArrayList(Vec3) = undefined,

    //----------------------------------------------------

    pub fn init(
        params: Params,
        renderer: RendererPtr,
        gameObject: GameObjectPtr,
        allocator: std.mem.Allocator,
    ) Self {
        var self = Self{
            .params = params,
            .path = CurvePath3D.init(allocator),
            .pathCalculator = CurvePathCalculator3D.init(allocator, params.maxCablePointCount),
            .smoothedPoints = std.ArrayList(Vec3).init(allocator),
        };

        // maxCablePointCount is the maximum number of input vertices that the cable
        // mesh is to follow. From it, we calculate the number of mesh vertices and
        // indices needed.

        const vertexCount = params.maxCablePointCount * VERTICES_PER_SAMPLED_POINT;
        const indexCount = (params.maxCablePointCount - 1) * 4 * 6;

        const mmData = RenderObject.ManualMeshData{
            .vertexFormatType = VERTEX_FORMAT_TYPE,
            .vertexCount = vertexCount,
            .indexCount = indexCount,
        };
        const meshSource = RenderObject.MeshSource{ .manualMesh = mmData };
        const parentNode: ?SceneNodePtr = null;

        self.renderObject = RenderObject.init(
            renderer,
            true,
            meshSource,
            params.materialResourcePath,
            parentNode,
            true,
            gameObject,
        );

        self.generateIndices();

        // No point in trying to render anything until we have set the geometry.
        self.renderObject.meshInstance.setVisible(false);

        return self;
    }

    pub fn deinit(self: *Self) void {
        self.renderObject.deinit();
        self.path.deinit();
        self.pathCalculator.deinit();
        self.smoothedPoints.deinit();
    }

    pub fn update(self: *Self, cablePoints: []const Vec3) !void {
        if (cablePoints.len < 2) {
            self.renderObject.meshInstance.setVisible(false);
            return;
        }

        // If params.smoothUsingCurvePath is true, we run the input
        // points through a curve path 3D to generate a smooth curve, which
        // we then sample to get the final mesh positions.
        const meshPoints = if (self.params.smoothUsingCurvePath) blk: {
            const fullPathLength = try self.pathCalculator.calculateFromPoints(&self.path, cablePoints);

            // We sample the curve at the resolution specified by params.maxCablePointCount.
            // However, if the cable is very short that resolution is waaay overkill, so we
            // specify a MIN_STEP_DISTANCE which each step at a minimum needs to advance
            // along the curve path when sampling from it.

            var t: f32 = 0.0;
            var curveSampleStep = 1.0 / @as(f32, @floatFromInt(self.params.maxCablePointCount - 1));

            const MIN_STEP_DISTANCE: f32 = 0.1;
            if (fullPathLength * curveSampleStep < MIN_STEP_DISTANCE) {
                curveSampleStep = 1.0 / (fullPathLength / MIN_STEP_DISTANCE);
            }

            self.smoothedPoints.clearRetainingCapacity();
            while (t < 1.0) : (t += curveSampleStep) {
                var curvePoint: Vec3 = undefined;
                var curveDir: Vec3 = undefined;

                self.path.sampleBezier(t, &curvePoint, &curveDir);

                try self.smoothedPoints.append(curvePoint);
            }

            // basis.printf("Path length: {d:.2}, step distance: {d:.2}, smooth point count: {}\n", .{
            //     fullPathLength,
            //     fullPathLength * curveSampleStep,
            //     self.smoothedPoints.items.len,
            // });

            break :blk self.smoothedPoints.items;
        } else cablePoints;

        //----------------------------------------------------

        const lod0 = self.renderObject.mesh.getLodLevel(0);
        const subMesh0 = lod0.getSubMesh(0);

        self.renderObject.meshInstance.setVisible(true);

        var up = Vec3.UnitY;
        var dir = Vec3.UnitY.negated();

        var vertices = subMesh0.getVertices();
        var stream = basis.BinaryWriteStream.init(vertices, true);

        self.bounds.clear();

        for (meshPoints, 0..) |p, i| {
            if (i < meshPoints.len - 1) {
                const nextP = meshPoints[i + 1];
                dir = (nextP.sub(p)).normalized();
            }

            var matrix: Mat43 = undefined;

            if (i == 0) {
                matrix = Mat43.fromLocalAxesPosition(Vec3.UnitX, Vec3.UnitZ, Vec3.UnitY, p);
            } else {
                matrix.lookToSafe(dir, up);
                matrix.setTranslation(p);
            }

            const thickness = self.params.cableThickness;

            var vertex: VERTEX_TYPE = undefined;

            // These are not set currently.
            vertex.tangent = Vec3.UnitX;
            vertex.binormal = Vec3.UnitX;
            vertex.texcoord = Vec2.Zero;

            // Up.
            vertex.position = matrix.transformPoint(Vec3.init(0.0, thickness, 0.0));
            vertex.normal = matrix.getY();
            stream.put(VERTEX_TYPE, vertex);
            self.bounds.addPoint(vertex.position);

            // Right.
            vertex.position = matrix.transformPoint(Vec3.init(thickness, 0.0, 0.0));
            vertex.normal = matrix.getX();
            stream.put(VERTEX_TYPE, vertex);
            self.bounds.addPoint(vertex.position);

            // Down.
            vertex.position = matrix.transformPoint(Vec3.init(0.0, -thickness, 0.0));
            vertex.normal = matrix.getY().negated();
            stream.put(VERTEX_TYPE, vertex);
            self.bounds.addPoint(vertex.position);

            // Left.
            vertex.position = matrix.transformPoint(Vec3.init(-thickness, 0.0, 0.0));
            vertex.normal = matrix.getX().negated();
            stream.put(VERTEX_TYPE, vertex);
            self.bounds.addPoint(vertex.position);

            // Use this point's local up-vector as the up-vector when constructing the next point.
            // This prevents the up (or down, left or right) points from suddenly appearing on the
            // opposite side of the spline.
            up = matrix.getY();
        }

        const pointCount = @as(u32, @intCast(meshPoints.len));

        subMesh0.setVertexCount(pointCount * 4);
        const indexCount = (pointCount - 1) * 4 * 6;
        subMesh0.setIndexCount(indexCount);

        lod0.setBounds(self.bounds);
        //self.bounds.debugDrawWithColor(basis.Color.Orange);
    }

    //----------------------------------------------------

    fn generateIndices(self: *Self) void {
        const lod0 = self.renderObject.mesh.getLodLevel(0);
        const subMesh0 = lod0.getSubMesh(0);

        var indices = subMesh0.getIndices();

        const stackCount = (self.params.maxCablePointCount - 1);

        var ii: u32 = 0;

        var i: u32 = 0;
        while (i < stackCount) : (i += 1) {
            var j: u32 = 0;
            while (j < VERTICES_PER_SAMPLED_POINT) : (j += 1) {
                if (j < VERTICES_PER_SAMPLED_POINT - 1) {
                    indices[ii + 0] = @intCast(i * VERTICES_PER_SAMPLED_POINT + j);
                    indices[ii + 1] = @intCast((i + 1) * VERTICES_PER_SAMPLED_POINT + j);
                    indices[ii + 2] = @intCast((i + 1) * VERTICES_PER_SAMPLED_POINT + j + 1);

                    indices[ii + 3] = @intCast(i * VERTICES_PER_SAMPLED_POINT + j);
                    indices[ii + 4] = @intCast((i + 1) * VERTICES_PER_SAMPLED_POINT + j + 1);
                    indices[ii + 5] = @intCast(i * VERTICES_PER_SAMPLED_POINT + j + 1);
                } else {
                    indices[ii + 0] = @intCast(i * VERTICES_PER_SAMPLED_POINT + j);
                    indices[ii + 1] = @intCast((i + 1) * VERTICES_PER_SAMPLED_POINT + j);
                    indices[ii + 2] = @intCast(((i + 1) * VERTICES_PER_SAMPLED_POINT + j + 1) - VERTICES_PER_SAMPLED_POINT);

                    indices[ii + 3] = @intCast(i * VERTICES_PER_SAMPLED_POINT + j);
                    indices[ii + 4] = @intCast(((i + 1) * VERTICES_PER_SAMPLED_POINT + j + 1) - VERTICES_PER_SAMPLED_POINT);
                    indices[ii + 5] = @intCast((i * VERTICES_PER_SAMPLED_POINT + j + 1) - VERTICES_PER_SAMPLED_POINT);
                }

                ii += 6;
            }
        }
    }
};

two_joint_crane_arm.zig

// MIT License
// 
// Copyright (c) 2018-2023 Madrigal Ltd.
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

const std = @import("std");
const basis = @import("basis");
const means = @import("../means.zig");

const AvatarTrackingComponent = basis.component_contexts.AvatarTrackingComponent;

const Vec2 = basis.math.Vec2;
const Vec3 = basis.math.Vec3;
const SceneNodePtr = basis.math.SceneNodePtr;
const Quaternion = basis.math.Quaternion;
const Mat43 = basis.math.Mat43;

const Color = basis.Color;

const PropagatedValue = basis.network.PropagatedValue;
const PropagatedValueHandle = basis.network.PropagatedValueHandle;

const RenderObject = means.render_object.RenderObject;

pub const TwoJointCraneArm = struct {
    const Self = @This();

    const GraphNodes = enum(usize) {
        Root = 0,
        Offset, // The idea behind the offset node is to allow the joint between the root and bone0 to be offset from the center of the root.
        Bone0,
        Bone1,
        ArmEnd,

        Count,

        pub fn idx(comptime self: GraphNodes) usize {
            return @intFromEnum(self);
        }
    };

    const RenderObjectCount = 3;

    pub const Params = struct {
        armRootPosition: Vec3 = Vec3.Zero,
        armBone0JointOffset: Vec3 = Vec3.Zero,
        armBone0Length: f32 = 0.0,
        armBone1Length: f32 = 0.0,
        restPoseRotation: f32 = 0.0,
        restPoseVerticalOffset: f32 = 0.0,
        restPoseHorizontalOffset: f32 = 0.0,
        craneRotationSpeed: f32 = 0.0,
        upDownSpeed: f32 = 0.0,
        forwardBackwardSpeed: f32 = 0.0,
    };

    //----------------------------------------------------

    context: AvatarTrackingComponent,

    params: Params = .{},

    serverOnlyParentNode: SceneNodePtr = SceneNodePtr.initNull(),

    graph: [GraphNodes.Count.idx()]SceneNodePtr =
        [_]SceneNodePtr{SceneNodePtr.initNull()} ** GraphNodes.Count.idx(),

    renderObjects: [RenderObjectCount]RenderObject = [_]RenderObject{.{}} ** RenderObjectCount,

    // X: Rotation around root
    // Y: Vertical distance
    // Z: Horizontal distance
    steeringValues: PropagatedValueHandle(Vec3),

    //----------------------------------------------------

    pub fn init(
        context: AvatarTrackingComponent,
    ) Self {
        return Self{
            .context = context,
            .steeringValues = PropagatedValue(Vec3).init(
                context,
                "crane_arm_steering_values",
                false,
                true,
                Vec3.init(0.0, 0.0, 0.0),
            ),
        };
    }

    pub fn create(self: *Self, params: Params) void {
        self.params = params;

        self.steeringValues.set(Vec3.init(params.restPoseRotation, params.restPoseVerticalOffset, params.restPoseHorizontalOffset));
        self.steeringValues.setValueChangedCallback(PropagatedValue(Vec3).Callback.initMethod(self, Self, onSteeringValuesUpdated));

        self.createGraph();

        self.applySteeringValues();

        if (self.context.onClient()) {
            self.createVisuals();
        }
    }

    pub fn destroy(self: *Self) void {
        if (self.context.onClient()) {
            self.destroyVisuals();
        }

        self.destroyGraph();
        self.steeringValues.deinit();
    }

    pub fn update(self: *Self, deltaTime: f32) void {
        _ = self;
        _ = deltaTime;
        //self.debugDraw();
    }

    pub fn preTick(
        self: *Self,
        tickDeltaTime: f32,
        rotationInput: f32,
        upDownInput: f32,
        forwardBackwardInput: f32,
    ) void {
        _ = tickDeltaTime;
        if (self.context.onServer()) {
            const steeringValues = self.steeringValues.get();
            var currentRotationAroundRoot = steeringValues.x;
            var currentVerticalDistance = steeringValues.y;
            var currentHorizontalDistance = steeringValues.z;
            var updated = false;

            if (!basis.math.floatsAlmostEqual(rotationInput, 0.0)) {
                const speed = self.params.craneRotationSpeed * std.math.fabs(rotationInput);

                //currentRotationAroundRoot += if (rotationInput > 0.0) speed else -speed;
                currentRotationAroundRoot += if (rotationInput > 0.0) -speed else speed; // Flipped

                if (currentRotationAroundRoot > basis.math.Pi) currentRotationAroundRoot -= basis.math.TwoPi;
                if (currentRotationAroundRoot < -basis.math.Pi) currentRotationAroundRoot += basis.math.TwoPi;

                updated = true;
            }

            if (!basis.math.floatsAlmostEqual(upDownInput, 0.0)) {
                const speed = self.params.upDownSpeed * std.math.fabs(upDownInput);
                currentVerticalDistance += if (upDownInput > 0.0) speed else -speed;
                updated = true;
            }

            if (!basis.math.floatsAlmostEqual(forwardBackwardInput, 0.0)) {
                const speed = self.params.forwardBackwardSpeed * std.math.fabs(forwardBackwardInput);
                currentHorizontalDistance += if (forwardBackwardInput > 0.0) speed else -speed;
                updated = true;
            }

            if (updated) {
                //basis.printf("Rot: {}, V: {}, H: {}\n", .{ currentRotationAroundRoot, currentVerticalDistance, currentHorizontalDistance });
                self.steeringValues.set(Vec3.init(currentRotationAroundRoot, currentVerticalDistance, currentHorizontalDistance));
            }
        }
    }

    pub fn tick(self: *Self, tickDeltaTime: f32) void {
        _ = tickDeltaTime;
        if (self.graph[GraphNodes.Root.idx()].isNull()) return;

        const worldMatrix = self.context.transform.getWorldMatrix();

        // On the client the serverOnlyParentNode is supposed to be null,
        // on the server, non-null.
        basis.assert(self.serverOnlyParentNode.isNull() == self.context.onClient());

        if (self.context.onServer()) {

            // On the server, the graph needs to be moved around with the parent
            // since it isn't nested under the render scene node.
            const pos = worldMatrix.getT();
            const ori = Quaternion.initFromRotationMatrix(worldMatrix);

            self.serverOnlyParentNode.setPosition(pos);
            self.serverOnlyParentNode.setOrientation(ori);
        }
    }

    //----------------------------------------------------

    fn debugDraw(self: *const Self) void {
        if (self.graph[GraphNodes.Root.idx()].isNull()) return;

        const rootPos = self.graph[GraphNodes.Root.idx()].getPositionInSpace(.World);
        const bone0Pos = self.graph[GraphNodes.Bone0.idx()].getPositionInSpace(.World);
        const bone1Pos = self.graph[GraphNodes.Bone1.idx()].getPositionInSpace(.World);
        const armEndPos = self.graph[GraphNodes.ArmEnd.idx()].getPositionInSpace(.World);

        if (self.context.onClient()) {
            basis.debug_draw.drawLine3D(rootPos, bone0Pos, Color.White);
            basis.debug_draw.drawLine3D(bone0Pos, bone1Pos, Color.Yellow);
            basis.debug_draw.drawLine3D(bone1Pos, armEndPos, Color.White);
            basis.debug_draw.drawSphere(rootPos, 1.0, Color.Red);
            basis.debug_draw.drawSphere(bone0Pos, 1.0, Color.Green);
            basis.debug_draw.drawSphere(bone1Pos, 1.0, Color.Blue);
            basis.debug_draw.drawSphere(armEndPos, 1.0, Color.White);
        } else {
            basis.debug_draw.drawLine3D(rootPos, bone0Pos, Color.Orange);
            basis.debug_draw.drawLine3D(bone0Pos, bone1Pos, Color.Orange);
            basis.debug_draw.drawLine3D(bone1Pos, armEndPos, Color.Orange);
            basis.debug_draw.drawSphere(rootPos, 1.0, Color.Orange);
            basis.debug_draw.drawSphere(bone0Pos, 1.0, Color.Orange);
            basis.debug_draw.drawSphere(bone1Pos, 1.0, Color.Orange);
            basis.debug_draw.drawSphere(armEndPos, 1.0, Color.Orange);
        }
    }

    pub fn getArmEndPos(self: *const Self) Vec3 {
        if (self.graph[GraphNodes.Root.idx()].isNull()) return Vec3.Zero;

        return self.graph[GraphNodes.ArmEnd.idx()].getPositionInSpace(.World);
    }

    //----------------------------------------------------

    fn createGraph(self: *Self) void {
        var graphParent = SceneNodePtr.initNull();

        if (self.context.onServer()) {
            // On the server, we create one extra scene node which is moved around with the parent object.
            self.serverOnlyParentNode = SceneNodePtr.initNew();
            graphParent = self.serverOnlyParentNode;
        } else {
            // On the client we attach the root node to the render scene node of the transform component.
            var renderSceneNode = self.context.transform.getRenderSceneNode();
            std.debug.assert(!renderSceneNode.isNull());
            graphParent = renderSceneNode;
        }

        self.graph[GraphNodes.Root.idx()] = graphParent.createChildNode();
        self.graph[GraphNodes.Root.idx()].setPosition(self.params.armRootPosition);

        self.graph[GraphNodes.Offset.idx()] = self.graph[GraphNodes.Root.idx()].createChildNode();
        self.graph[GraphNodes.Offset.idx()].setPosition(self.params.armBone0JointOffset);

        self.graph[GraphNodes.Bone0.idx()] = self.graph[GraphNodes.Offset.idx()].createChildNode();
        //self.graph[GraphNodes.Bone0.idx()].setPosition(Vec3.init(0, 0, self.params.armBone0Length));

        self.graph[GraphNodes.Bone1.idx()] = self.graph[GraphNodes.Bone0.idx()].createChildNode();
        self.graph[GraphNodes.Bone1.idx()].setPosition(Vec3.init(0, 0, self.params.armBone0Length));

        self.graph[GraphNodes.ArmEnd.idx()] = self.graph[GraphNodes.Bone1.idx()].createChildNode();
        self.graph[GraphNodes.ArmEnd.idx()].setPosition(Vec3.init(0, 0, self.params.armBone1Length));
    }

    fn destroyGraph(self: *Self) void {
        if (self.context.onServer()) {
            self.serverOnlyParentNode.deinit(); // On the server we delete the entire node graph.
            self.serverOnlyParentNode = SceneNodePtr.initNull();
        } else {
            var renderSceneNode = self.context.transform.getRenderSceneNode();
            std.debug.assert(!renderSceneNode.isNull());
            renderSceneNode.destroyChildNode(self.graph[GraphNodes.Root.idx()]);
        }

        for (&self.graph) |*node| {
            node.* = SceneNodePtr.initNull();
        }
    }

    fn createVisuals(self: *Self) void {
        const renderer = self.context.getRenderer();
        const go = self.context.getGameObject();
        const createDedicatedNode = false;

        // TODO: Put the mesh/material resource paths in the params, so that they can be overridden.

        {
            const meshSource = RenderObject.MeshSource{ .meshResourcePath = "means/vehicles/cargo_crane/crane_arm_base_blockout.binmesh" };
            const materialPath = "means/vehicles/cargo_crane/arm_blockout.binmaterial";
            const parentNode = self.graph[GraphNodes.Root.idx()];

            self.renderObjects[0] = RenderObject.init(renderer, true, meshSource, materialPath, parentNode, createDedicatedNode, go);
        }

        {
            const meshSource = RenderObject.MeshSource{ .meshResourcePath = "means/vehicles/cargo_crane/crane_arm_bone0_blockout.binmesh" };
            const materialPath = "means/vehicles/cargo_crane/arm_blockout.binmaterial";
            const parentNode = self.graph[GraphNodes.Bone0.idx()];

            self.renderObjects[1] = RenderObject.init(renderer, true, meshSource, materialPath, parentNode, createDedicatedNode, go);
        }

        {
            const meshSource = RenderObject.MeshSource{ .meshResourcePath = "means/vehicles/cargo_crane/crane_arm_bone1_blockout.binmesh" };
            const materialPath = "means/vehicles/cargo_crane/arm_blockout.binmaterial";
            const parentNode = self.graph[GraphNodes.Bone1.idx()];

            self.renderObjects[2] = RenderObject.init(renderer, true, meshSource, materialPath, parentNode, createDedicatedNode, go);
        }
    }

    fn destroyVisuals(self: *Self) void {
        for (&self.renderObjects) |*ro| {
            ro.deinit();
        }
    }

    fn onSteeringValuesUpdated(self: *Self, steeringValues: Vec3, localChange: bool, valueTime: f64) void {
        _ = steeringValues;
        _ = valueTime;
        _ = localChange;

        self.applySteeringValues();
    }

    fn applySteeringValues(self: *Self) void {
        const steeringValues = self.steeringValues.get();
        const rotationAroundRoot = steeringValues.x;
        const ikTarget = Vec2.init(steeringValues.y, steeringValues.z);

        {
            var rot: Quaternion = Quaternion.Identity;
            rot.setRotationY(rotationAroundRoot);
            self.graph[GraphNodes.Root.idx()].setOrientationInSpace(rot, .Parent, true);
        }

        if (basis.math.vec2sAlmostEqual(ikTarget, Vec2.Zero)) {
            return; // We don't have any IK positions yet.
        }

        // ikTarget.x is vertical distance, ikTarget.y is horizontal distance
        //BASIS_PRINTF("z: %.2f,y: %.2f\n", ikTarget.x, ikTarget.y);

        const length0 = self.params.armBone0Length;
        const length1 = self.params.armBone1Length;

        var jointAngle0: f32 = 0.0;
        var jointAngle1: f32 = 0.0;

        const length2 = ikTarget.length();

        // Angle from Joint0 and Target
        //const atan = std.math.atan2(f32, ikTarget.y, ikTarget.x);
        const atan = std.math.atan2(f32, ikTarget.x, ikTarget.y);

        // Is the target reachable?
        // If not, we stretch as far as possible
        if (length0 + length1 < length2) {
            jointAngle0 = atan;
            jointAngle1 = 0.0;
        } else {
            const cosAngle0 = ((length2 * length2) + (length0 * length0) - (length1 * length1)) / (2.0 * length2 * length0);
            const angle0 = std.math.acos(cosAngle0);

            const cosAngle1 = ((length1 * length1) + (length0 * length0) - (length2 * length2)) / (2.0 * length1 * length0);
            const angle1 = std.math.acos(cosAngle1);

            jointAngle0 = atan + angle0;
            jointAngle1 = basis.math.Pi - angle1;
        }

        {
            var q: Quaternion = Quaternion.Identity;
            q.setRotationX(jointAngle0);
            //self.graph[GraphNodes.Bone0.idx()].setOrientation(q);
            self.graph[GraphNodes.Bone0.idx()].setOrientationInSpace(q, .Parent, true);
        }

        {
            var q: Quaternion = Quaternion.Identity;
            q.setRotationX(-jointAngle1);
            //self.graph[GraphNodes.Bone1.idx()].setOrientation(q);
            self.graph[GraphNodes.Bone1.idx()].setOrientationInSpace(q, .Parent, true);
        }
    }
};