KavTest/KavTest/Engines/MotionEngine.cs

using Encompass;
using KavTest.Components;
using KavTest.Messages;
using Microsoft.Xna.Framework;

namespace KavTest.Engines
{
    [Reads(typeof(Transform3DComponent))]
    [Receives(
        typeof(TranslationMessage),
        typeof(LocalTranslationMessage),
        typeof(LocalRotationMessage)
    )]
    [Writes(typeof(Transform3DComponent))]
    [QueryWith(typeof(Transform3DComponent))]
    [QueryWithout(typeof(StaticComponent))]
    public class MotionEngine : Engine
    {
        public override void Update(double dt)
        {
            foreach (var entity in TrackedEntities)
            {
                ref readonly var transformComponent = ref GetComponent<Transform3DComponent>(entity);

                Vector3 translation = Vector3.Zero;
                Vector3 localTranslation = Vector3.Zero;

                Quaternion rotation = Quaternion.Identity;
                Quaternion localRotation = Quaternion.Identity;

                foreach (var message in ReadMessagesWithEntity<TranslationMessage>(entity))
                {
                    translation += message.Translation;
                }

                foreach (var message in ReadMessagesWithEntity<LocalTranslationMessage>(entity))
                {
                    localTranslation += message.Translation;
                }

                foreach (var message in ReadMessagesWithEntity<LocalRotationMessage>(entity))
                {
                    localRotation *= message.Rotation;
                }

                var newTransform =
                    transformComponent.Transform.
                    TranslateLocal(localTranslation).
                    RotateLocal(localRotation).
                    Translate(translation);


                SetComponent(entity, new Transform3DComponent(newTransform));
            }
        }
    }
}