hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/OpenScadModel.cs

using System.Collections.Generic;
using System.IO;
using UnityEngine;
using UnityEngine.Rendering;

namespace HexapodSim
{
    // Swaps the old scene's robot for the OpenSCAD model (hardware/openscad,
    // exported by export_unity.py into Resources/OpenScadModel). Builds a new
    // rig under the same chassis with the names HexapodSimulation looks for
    // (Leg > hip1 > Hip2 > Knie, plus the Point foot marker), with its pivots
    // exactly at the Config.h geometry, and hides the old visuals. The meshes
    // are in the rest pose (coxa 0, femur 0, tibia -90), which is the rig's
    // zero rotation, so the existing joint driving moves them correctly.
    public static class OpenScadModel
    {
        const string Folder = "OpenScadModel/";

        public static bool Available => Resources.Load<TextAsset>(Folder + "body") != null;

        public static void Build(Transform chassis, HxGeometry g, ForwardAxis f, float unitsPerMm)
        {
            Material template = null;
            foreach (var r in chassis.GetComponentsInChildren<Renderer>(true))
            {
                if (template == null && r.sharedMaterial) template = r.sharedMaterial;
                r.enabled = false;
            }
            foreach (var t in Object.FindObjectsByType<Transform>())
                if (t.name == "Leg" && HasChild(t, "hip1")) t.gameObject.SetActive(false);

            var shader = Shader.Find("Standard") ?? (template ? template.shader : null);
            AddMesh(chassis, "OpenSCAD body", Folder + "body", Quaternion.identity, f, unitsPerMm, shader);

            for (int i = 0; i < g.numLegs; i++)
            {
                string side = g.mountAngleDeg[i] > 0 ? "left" : "right";
                float a = g.mountAngleDeg[i] * Mathf.Deg2Rad;
                var root = new GameObject("Leg").transform;
                root.SetParent(chassis, false);
                root.localPosition = RobotFrame.ToUnity(new Vector3(g.mountXmm[i], g.mountYmm[i], 0), f) * unitsPerMm;
                root.localRotation = Quaternion.LookRotation(
                    RobotFrame.ToUnity(new Vector3(Mathf.Cos(a), Mathf.Sin(a), 0), f), Vector3.up);
                // The meshes are exported as a leg pointing straight sideways (mount
                // +90 left, -90 right) in chassis axes; in the joint's own frame that
                // is the same for any mount angle.
                float refRad = (side == "left" ? 90f : -90f) * Mathf.Deg2Rad;
                Quaternion toJoint = Quaternion.Inverse(Quaternion.LookRotation(
                    RobotFrame.ToUnity(new Vector3(Mathf.Cos(refRad), Mathf.Sin(refRad), 0), f), Vector3.up));

                var hip1 = Child(root, "hip1", Vector3.zero);
                AddMesh(hip1, "coxa", Folder + $"leg_{side}_coxa", toJoint, f, unitsPerMm, shader);
                var hip2 = Child(hip1, "Hip2", new Vector3(0, 0, g.coxaMm * unitsPerMm));
                AddMesh(hip2, "femur", Folder + $"leg_{side}_femur", toJoint, f, unitsPerMm, shader);
                var knie = Child(hip2, "Knie", new Vector3(0, 0, g.femurMm * unitsPerMm));
                AddMesh(knie, "tibia", Folder + $"leg_{side}_tibia", toJoint, f, unitsPerMm, shader);

                // Where the firmware's FK puts the foot (centre of the foot ball).
                var marker = GameObject.CreatePrimitive(PrimitiveType.Sphere);
                var collider = marker.GetComponent<Collider>();
                if (Application.isPlaying) Object.Destroy(collider); else Object.DestroyImmediate(collider);
                marker.name = "Point";
                marker.transform.SetParent(knie, false);
                marker.transform.localScale = Vector3.one * 4f * unitsPerMm;
                marker.GetComponent<Renderer>().material = MakeMaterial(shader, new Color(0.9f, 0.1f, 0.1f));
            }
        }

        static bool HasChild(Transform t, string name)
        {
            foreach (Transform c in t)
                if (string.Equals(c.name, name, System.StringComparison.OrdinalIgnoreCase) || HasChild(c, name)) return true;
            return false;
        }

        static Transform Child(Transform parent, string name, Vector3 localPos)
        {
            var t = new GameObject(name).transform;
            t.SetParent(parent, false);
            t.localPosition = localPos;
            return t;
        }

        static Material MakeMaterial(Shader shader, Color c)
        {
            var m = new Material(shader) { color = c };
            if (m.HasProperty("_Glossiness")) m.SetFloat("_Glossiness", 0.25f);
            return m;
        }

        // Reads one export_unity.py file: robot-frame mm, pivot-relative,
        // counter-clockwise outward winding (right-handed). RobotFrame.ToUnity is a
        // reflection, so every triangle is flipped to stay front-facing.
        static void AddMesh(Transform parent, string name, string resource, Quaternion toLocal,
                            ForwardAxis f, float unitsPerMm, Shader shader)
        {
            var asset = Resources.Load<TextAsset>(resource);
            if (asset == null) { Debug.LogWarning($"[HexapodSim] missing model mesh {resource}"); return; }

            var verts = new List<Vector3>();
            var groups = new List<int[]>();
            var mats = new List<Material>();
            using (var br = new BinaryReader(new MemoryStream(asset.bytes)))
            {
                if (new string(br.ReadChars(4)) != "HXM1") { Debug.LogWarning($"[HexapodSim] {resource}: not a model mesh"); return; }
                int n = br.ReadInt32();
                for (int k = 0; k < n; k++)
                {
                    var col = new Color(br.ReadSingle(), br.ReadSingle(), br.ReadSingle(), br.ReadSingle());
                    int tris = br.ReadInt32();
                    var idx = new int[tris * 3];
                    for (int t = 0; t < tris; t++)
                    {
                        int b0 = verts.Count;
                        for (int v = 0; v < 3; v++)
                        {
                            var r = new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle());
                            verts.Add(toLocal * (RobotFrame.ToUnity(r, f) * unitsPerMm));
                        }
                        idx[t * 3] = b0; idx[t * 3 + 1] = b0 + 2; idx[t * 3 + 2] = b0 + 1;
                    }
                    groups.Add(idx);
                    mats.Add(MakeMaterial(shader, col));
                }
            }

            var mesh = new Mesh { name = name, indexFormat = IndexFormat.UInt32 };
            mesh.SetVertices(verts);
            mesh.subMeshCount = groups.Count;
            for (int k = 0; k < groups.Count; k++) mesh.SetTriangles(groups[k], k);
            mesh.RecalculateNormals();
            mesh.RecalculateBounds();

            var go = new GameObject(name);
            go.transform.SetParent(parent, false);
            go.AddComponent<MeshFilter>().sharedMesh = mesh;
            go.AddComponent<MeshRenderer>().sharedMaterials = mats.ToArray();
        }
    }
}