Nerfed/Nerfed.Runtime/Util/Transform.cs

using MoonTools.ECS;
using Nerfed.Runtime.Components;
using System.Collections.Generic;
using System.Numerics;

namespace Nerfed.Runtime.Util
{
    // https://github.com/needle-mirror/com.unity.entities/blob/master/Unity.Transforms/TransformHelpers.cs
    public static class Transform
    {
        public static Vector3 Forward(in this Matrix4x4 matrix) => new Vector3(matrix.M31, matrix.M32, matrix.M33);
        public static Vector3 Back(in this Matrix4x4 matrix) => -matrix.Forward();
        public static Vector3 Up(in this Matrix4x4 matrix) => new Vector3(matrix.M21, matrix.M22, matrix.M23);
        public static Vector3 Down(in this Matrix4x4 matrix) => -matrix.Up();
        public static Vector3 Right(in this Matrix4x4 matrix) => new Vector3(matrix.M11, matrix.M12, matrix.M13);
        public static Vector3 Left(in this Matrix4x4 matrix) => -matrix.Right();
        //public static Vector3 Translation(in this Matrix4x4 matrix) => new Vector3();
        //public static Quaternion Rotation(in this Matrix4x4 matrix) => new Quaternion();

        public static Matrix4x4 TRS(in this LocalTransform localTransform)
        {
            return Matrix4x4.CreateScale(localTransform.scale) *
                Matrix4x4.CreateFromQuaternion(localTransform.rotation) *
                Matrix4x4.CreateTranslation(localTransform.position);
        }

        // Sets the parent child relation and adds a child component.
        // Relation goes from child to parent.
        public static void SetParent(in World world, in Entity child, in Entity parent)
        {
            RemoveParent(world, child);

            world.Relate(child, parent, new ChildParentRelation());
            world.Set(child, new Child());
            world.Remove<Root>(child);
            return;
        }

        // Removes any parent child relation ship, thus making it a 'root' object.
        public static void RemoveParent(in World world, in Entity child)
        {
            if (!world.HasOutRelation<ChildParentRelation>(child))
            {
                return;
            }

            Entity parent = world.OutRelationSingleton<ChildParentRelation>(child);

            // TODO: Check if Unrelate all also unrelates incomming relations..?
            world.Unrelate<ChildParentRelation>(child, parent);
            world.Remove<Child>(child);
            world.Set(child, new Root());
        }

        public static Entity CreateBaseEntity(this World world, string tag = "")
        {
            Entity entity = world.CreateEntity(tag);
            world.Set(entity, new Root());
            return entity;
        }

        // Force update the transform data of an entity (and children).
        // Useful for when you need precise up to date transform data.
        public static void ForceUpdateLocalToWorld(in World world, in Entity entity)
        {
            Matrix4x4 parentLocalToWorldMatrix = Matrix4x4.Identity;

            if (world.HasOutRelation<ChildParentRelation>(entity)) {
                Entity parent = world.OutRelationSingleton<ChildParentRelation>(entity);

                if (world.Has<LocalToWorld>(parent))
                {
                    parentLocalToWorldMatrix = world.Get<LocalToWorld>(parent).localToWorldMatrix;
                }
            }

            ForceUpdateLocalToWorld(world, entity, parentLocalToWorldMatrix);
        }

        private static void ForceUpdateLocalToWorld(in World world, in Entity entity, Matrix4x4 localToWorldMatrix)
        {
            if (world.Has<LocalTransform>(entity))
            {
                LocalTransform localTransform = world.Get<LocalTransform>(entity);
                localToWorldMatrix = Matrix4x4.Multiply(localToWorldMatrix, localTransform.TRS());
                LocalToWorld localToWorld = new(localToWorldMatrix);
                world.Set(entity, localToWorld);

                Log.Info($"Entity {entity} | local position {localTransform.position} | world position {localToWorldMatrix.Translation}");
            }

            ReverseSpanEnumerator<Entity> childEntities = world.InRelations<ChildParentRelation>(entity);
            foreach (Entity childEntity in childEntities)
            {
                ForceUpdateLocalToWorld(world, childEntity, localToWorldMatrix);
            }
        }
    }
}