Add GPU skinning in RGL (#345)

* Adjust Plugin to new RGL version

* Make skinned models readable

* Add new API calls

* Perform skinning in RGL

* Optimize calculating the pose by caching it for objects that share the same skeleton

* Cleanup & docs

* Update RGL binaries

* Remove unused package import

* Review fixes

* Update RGL binary for Linux

Assets/RGLUnityPlugin/Scripts/LidarSensor.cs

@@ -131,7 +131,7 @@ public void Awake()
 
         public void Start()
         {
-            sceneManager = FindObjectOfType<SceneManager>();
+            sceneManager = SceneManager.Instance;
             if (sceneManager == null)
             {
                 // TODO(prybicki): this is too tedious, implement automatic instantiation of RGL Scene Manager

Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs

@@ -0,0 +1,112 @@
+// Copyright 2024 Robotec.ai.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+using System;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace RGLUnityPlugin
+{
+    /// <summary>
+    /// Since multiple skinned meshes can share the same skeleton, BonesPoseCacheManager improves the performance of the simulation
+    /// by caching the calculated pose of the skeletons for the given simulation step.
+    /// </summary>
+    public static class BonesPoseCacheManager
+    {
+        private class BonesPose
+        {
+            public BonesPose(SkinnedMeshRenderer smr)
+            {
+                this.smr = smr;
+                var bonesCount = smr.bones.Length;
+                pose = new Matrix4x4[bonesCount];
+                rglPose = new float[bonesCount * 3 * 4]; // Mat3x4 == 12 floats
+                usageCount = 1;
+            }
+
+            public SkinnedMeshRenderer smr; // needed to retrieve the current skeleton's pose
+            public Matrix4x4[] pose; // buffer for the skeleton's pose in the Unity representation
+            public float[] rglPose; // buffer for the skeleton's pose in the RGL representation
+            public int usageCount; // the counter on how many meshes use this skeleton
+            public int lastUpdateFrame = -1; // simulation frame
+            public int lastFixedUpdateFrame = -1; // physics cycle in the simulation frame
+        }
+
+        // The bone root acts as the identifier of the skeleton.
+        private static Dictionary<Transform, BonesPose> boneRootToBonesPose = new Dictionary<Transform, BonesPose>();
+
+        public static void RegisterBonesPoseInstance(SkinnedMeshRenderer smr)
+        {
+            if (!boneRootToBonesPose.ContainsKey(smr.rootBone))
+            {
+                boneRootToBonesPose.Add(smr.rootBone, new BonesPose(smr));
+            }
+            else
+            {
+                boneRootToBonesPose[smr.rootBone].usageCount++;
+            }
+        }
+
+        public static void UnregisterBonesPoseInstance(Transform rootBone)
+        {
+            if (!boneRootToBonesPose.ContainsKey(rootBone))
+            {
+                Debug.LogWarning(
+                    $"Trying to unregister absent in BonesPoseCacheManager rootBone: '{rootBone.name}', ignoring request");
+                return;
+            }
+
+            var bonesPose = boneRootToBonesPose[rootBone];
+            bonesPose.usageCount--;
+            if (bonesPose.usageCount == 0)
+            {
+                boneRootToBonesPose.Remove(rootBone);
+            }
+        }
+
+        public static float[] GetRglPose(Transform rootBone)
+        {
+            if (!boneRootToBonesPose.ContainsKey(rootBone))
+            {
+                throw new NotSupportedException(
+                    $"Trying to get RglPose from root bone ('{rootBone.name}') that is not registered in BonesPoseCacheManager");
+            }
+
+            var bonesPose = boneRootToBonesPose[rootBone];
+
+            if (SceneManager.Instance.LastUpdateFrame == bonesPose.lastUpdateFrame &&
+                SceneManager.Instance.LastFixedUpdateFrame == bonesPose.lastFixedUpdateFrame)
+            {
+                return bonesPose.rglPose;
+            }
+
+            var bones = bonesPose.smr.bones;
+            for (int i = 0; i < bonesPose.pose.Length; i++)
+            {
+                bonesPose.pose[i] = bones[i].localToWorldMatrix;
+            }
+
+            RGLNativeAPI.IntoMat3x4f(bonesPose.pose, ref bonesPose.rglPose);
+
+            bonesPose.lastUpdateFrame = SceneManager.Instance.LastUpdateFrame;
+            bonesPose.lastFixedUpdateFrame = SceneManager.Instance.LastFixedUpdateFrame;
+            return bonesPose.rglPose;
+        }
+
+        public static void Clear()
+        {
+            boneRootToBonesPose.Clear();
+        }
+    }
+}

Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs

@@ -79,15 +79,15 @@ public abstract class RGLObject<T> : IRGLObject
     {
         private readonly string identifier;
         private RGLTexture rglTexture;
-        private IntPtr rglEntityPtr;
 
+        protected IntPtr rglEntityPtr;
         protected RGLMesh rglMesh;
 
         public GameObject RepresentedGO { get; }
         public int? CategoryId { get; private set; }
         public string CategoryName { get; private set; }
 
-        // There are different stratiegies for obtaining a RGLMesh so we have to also destroy it differently.
+        // There are different strategies for obtaining a RGLMesh so we have to also destroy it differently.
         protected abstract RGLMesh GetRGLMeshFrom(T meshSource);
         protected abstract void DestroyRGLMesh();
 
@@ -134,13 +134,9 @@ public virtual void DestroyInRGL()
             }
         }
 
-        public void Update()
+        public virtual void Update()
         {
             UpdateTransform();
-            if (rglMesh is RGLSkinnedMesh rglSkinnedMesh)
-            {
-                rglSkinnedMesh.UpdateSkinnedMesh();
-            }
         }
 
         protected virtual void UpdateTransform()
@@ -159,7 +155,7 @@ protected virtual void UpdateTransform()
                 fixed (float* pMatrix3x4 = matrix3x4)
                 {
                     RGLNativeAPI.CheckErr(
-                        RGLNativeAPI.rgl_entity_set_pose(rglEntityPtr, (IntPtr) pMatrix3x4));
+                        RGLNativeAPI.rgl_entity_set_transform(rglEntityPtr, (IntPtr) pMatrix3x4));
                 }
             }
         }
@@ -278,6 +274,8 @@ protected override void DestroyRGLMesh()
     public class RGLSkinnedMeshRendererObject : RGLObject<SkinnedMeshRenderer>
     {
         private readonly Transform skinnedMeshRendererTransform;
+        private readonly Transform rootBone;
+        private readonly int bonesCount;
 
         public RGLSkinnedMeshRendererObject(SkinnedMeshRenderer skinnedMeshRenderer) :
             base(
@@ -287,6 +285,11 @@ public RGLSkinnedMeshRendererObject(SkinnedMeshRenderer skinnedMeshRenderer) :
                 )
         {
             skinnedMeshRendererTransform = skinnedMeshRenderer.transform;
+            rootBone = skinnedMeshRenderer.rootBone; // Identifier of the skeleton.
+            bonesCount = skinnedMeshRenderer.bones.Length;
+            // Multiple skinned meshes can share the same skeleton.
+            // To improve performance BonesPoseCacheManager will cache the last calculated pose of the skeletons.
+            BonesPoseCacheManager.RegisterBonesPoseInstance(skinnedMeshRenderer);
         }
 
         protected override RGLMesh GetRGLMeshFrom(SkinnedMeshRenderer skinnedMeshRenderer)
@@ -295,18 +298,41 @@ protected override RGLMesh GetRGLMeshFrom(SkinnedMeshRenderer skinnedMeshRendere
             {
                 throw new NotSupportedException($"Shared skinned mesh of '{skinnedMeshRenderer.gameObject}' is null");
             }
-            // Skinned meshes cannot be shared by using RGLMeshSharingManager
-            return new RGLSkinnedMesh(skinnedMeshRenderer.gameObject.GetInstanceID(), skinnedMeshRenderer);
+            var outRglMesh = RGLMeshSharingManager.RegisterRGLMeshInstance(skinnedMeshRenderer.sharedMesh);
+            // Bone weights and bindposes need to be uploaded to perform skeleton animation.
+            outRglMesh.UploadBoneWeights();
+            outRglMesh.UploadBindposes();
+            return outRglMesh;
+        }
+
+        // Override base class Update() method that sets world's transform.
+        // Instead, set the pose of the skeleton in the world coordinates. RGL will perform skeleton animation on GPU.
+        public override void Update()
+        {
+            unsafe
+            {
+                fixed (float* pPoseFloats = BonesPoseCacheManager.GetRglPose(rootBone))
+                {
+                    RGLNativeAPI.CheckErr(
+                        RGLNativeAPI.rgl_entity_set_pose_world(rglEntityPtr, (IntPtr)pPoseFloats, bonesCount));
+                }
+            }
         }
 
         protected override Matrix4x4 GetLocalToWorld()
         {
             return skinnedMeshRendererTransform.localToWorldMatrix;
         }
-
+
+        public override void DestroyInRGL()
+        {
+            base.DestroyInRGL();
+            BonesPoseCacheManager.UnregisterBonesPoseInstance(rootBone);
+        }
+
         protected override void DestroyRGLMesh()
         {
-            rglMesh.DestroyInRGL();
+            RGLMeshSharingManager.UnregisterRGLMeshInstance(rglMesh);
         }
     }
 
@@ -536,38 +562,44 @@ public void UploadUVs()
                }
             }
         }
-    }
 
-    /// <summary>
-    /// Some objects (such as NPC) use skinned meshes, which needs to be constantly updated by the Unity side.
-    /// </summary>
-    public class RGLSkinnedMesh : RGLMesh
-    {
-        private readonly SkinnedMeshRenderer skinnedMeshRenderer;
-
-        public RGLSkinnedMesh(int identifier, SkinnedMeshRenderer smr)
+        public void UploadBoneWeights()
         {
-            Identifier = identifier;
-            Mesh = new Mesh();
-            skinnedMeshRenderer = smr;
-            skinnedMeshRenderer.BakeMesh(Mesh, true);
-            UploadToRGL();
+            var boneWeights = Mesh.boneWeights;
+            bool boneWeightsOK = boneWeights != null && boneWeights.Length > 0;
+            if (!boneWeightsOK)
+            {
+                throw new NotSupportedException(
+                    $"Could not get bone weights from Mesh '{Mesh.name}'. The mesh may be not adapted for animation.");
+            }
+
+            unsafe
+            {
+                fixed (BoneWeight* pBoneWeights = boneWeights)
+                {
+                    RGLNativeAPI.CheckErr(
+                        RGLNativeAPI.rgl_mesh_set_bone_weights(RGLMeshPtr, (IntPtr) pBoneWeights, Mesh.boneWeights.Length));
+                }
+            }
         }
 
-        public void UpdateSkinnedMesh()
+        public void UploadBindposes()
         {
-            skinnedMeshRenderer.BakeMesh(Mesh, true);
+            var bindposes = Mesh.bindposes;
+            bool bindposesOK = bindposes != null && bindposes.Length > 0;
+            if (!bindposesOK)
+            {
+                throw new NotSupportedException(
+                    $"Could not get bindposes from Mesh '{Mesh.name}'. The mesh may be not adapted for animation.");
+            }
+
+            var bindposesFloats = RGLNativeAPI.IntoMat3x4f(bindposes);
             unsafe
             {
-                // Accessing .vertices perform a CPU copy!
-                // TODO: This could be optimized using Vulkan-CUDA interop and Unity NativePluginInterface. Expect difficulties.
-                // https://docs.unity3d.com/ScriptReference/Mesh.GetNativeVertexBufferPtr.html
-                // https://docs.unity3d.com/Manual/NativePluginInterface.html
-                // https://github.com/NVIDIA/cuda-samples/tree/master/Samples/5_Domain_Specific/simpleVulkan
-                fixed (Vector3* pVertices = Mesh.vertices)
+                fixed (float* pBindposesFloats = bindposesFloats)
                 {
                     RGLNativeAPI.CheckErr(
-                        RGLNativeAPI.rgl_mesh_update_vertices(RGLMeshPtr, (IntPtr) pVertices, Mesh.vertices.Length));
+                        RGLNativeAPI.rgl_mesh_set_restposes(RGLMeshPtr, (IntPtr) pBindposesFloats, Mesh.bindposes.Length));
                 }
             }
         }

Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs

@@ -40,10 +40,13 @@ public static class RGLNativeAPI
         public static extern int rgl_mesh_set_texture_coords(IntPtr mesh, IntPtr uvs, int uvCount);
 
         [DllImport("RobotecGPULidar")]
-        public static extern int rgl_mesh_destroy(IntPtr mesh);
+        public static extern int rgl_mesh_set_bone_weights(IntPtr mesh, IntPtr bone_weights, int bone_weights_count);
+
+        [DllImport("RobotecGPULidar")]
+        public static extern int rgl_mesh_set_restposes(IntPtr mesh, IntPtr restposes, int bones_count);
 
         [DllImport("RobotecGPULidar")]
-        public static extern int rgl_mesh_update_vertices(IntPtr mesh, IntPtr vertices, int vertex_count);
+        public static extern int rgl_mesh_destroy(IntPtr mesh);
 
         [DllImport("RobotecGPULidar")]
         public static extern int rgl_entity_create(out IntPtr entity, IntPtr scene, IntPtr mesh);
@@ -52,14 +55,20 @@ public static class RGLNativeAPI
         public static extern int rgl_entity_destroy(IntPtr entity);
 
         [DllImport("RobotecGPULidar")]
-        public static extern int rgl_entity_set_pose(IntPtr entity, IntPtr local_to_world_tf);
+        public static extern int rgl_entity_set_transform(IntPtr entity, IntPtr local_to_world_tf);
+
+        [DllImport("RobotecGPULidar")]
+        public static extern int rgl_entity_set_pose_world(IntPtr entity, IntPtr pose, int bones_count);
 
         [DllImport("RobotecGPULidar")]
         public static extern int rgl_entity_set_id(IntPtr entity, int id);
 
         [DllImport("RobotecGPULidar")]
         public static extern int rgl_entity_set_intensity_texture(IntPtr entity, IntPtr texture);
 
+        [DllImport("RobotecGPULidar")]
+        public static extern int rgl_entity_apply_external_animation(IntPtr entity, IntPtr vertices, int vertex_count);
+
         [DllImport("RobotecGPULidar")]
         public static extern int rgl_texture_create(out IntPtr texture, IntPtr texels, int width, int height);
 
@@ -329,6 +338,16 @@ public static float[] IntoVec3f(Vector3 vec)
         public static float[] IntoMat3x4f(Matrix4x4[] mats)
         {
             var matFloats = new float[mats.Length * 12];
+            IntoMat3x4f(mats, ref matFloats);
+            return matFloats;
+        }
+
+        public static void IntoMat3x4f(Matrix4x4[] mats, ref float[] outFloats)
+        {
+            if (outFloats == null || outFloats.Length != mats.Length * 12)
+            {
+                outFloats = new float[mats.Length * 12];
+            }
 
             for (int i = 0; i < mats.Length; ++i)
             {
@@ -337,12 +356,10 @@ public static float[] IntoMat3x4f(Matrix4x4[] mats)
                     for (int col = 0; col < 4; col++)
                     {
                         int idx = 12 * i + 4 * row + col;
-                        matFloats[idx] = mats[i][row, col];
+                        outFloats[idx] = mats[i][row, col];
                     }
                 }
             }
-
-            return matFloats;
         }
 
         public static float[] IntoMat3x4f(Matrix4x4 mat)

Assets/RGLUnityPlugin/Scripts/RadarSensor.cs

@@ -98,7 +98,7 @@ public void Awake()
 
         public void Start()
         {
-            sceneManager = FindObjectOfType<SceneManager>();
+            sceneManager = SceneManager.Instance;
             if (sceneManager == null)
             {
                 // TODO(prybicki): this is too tedious, implement automatic instantiation of RGL Scene Manager