diff --git a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta index d5811923d..622d77450 100644 --- a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta +++ b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta @@ -253,7 +253,7 @@ ModelImporter: maskType: 3 maskSource: {instanceID: 0} additiveReferencePoseFrame: 0 - isReadable: 0 + isReadable: 1 meshes: lODScreenPercentages: - 0.25 diff --git a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta index 9a61ac6e1..b6518d4c7 100644 --- a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta +++ b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta @@ -253,7 +253,7 @@ ModelImporter: maskType: 3 maskSource: {instanceID: 0} additiveReferencePoseFrame: 0 - isReadable: 0 + isReadable: 1 meshes: lODScreenPercentages: - 0.25 diff --git a/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so b/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so index 316bb5a5a..2325e899f 100755 Binary files a/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so and b/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so differ diff --git a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs index 354c79a4a..929864acc 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs @@ -131,7 +131,7 @@ public void Awake() public void Start() { - sceneManager = FindObjectOfType(); + sceneManager = SceneManager.Instance; if (sceneManager == null) { // TODO(prybicki): this is too tedious, implement automatic instantiation of RGL Scene Manager diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs new file mode 100644 index 000000000..0a64fff9b --- /dev/null +++ b/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 +{ + /// + /// 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. + /// + 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 boneRootToBonesPose = new Dictionary(); + + 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(); + } + } +} diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs.meta b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs.meta new file mode 100644 index 000000000..e16483dd9 --- /dev/null +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: f2d073eeb8da43b6be986a5e5b65fb51 +timeCreated: 1723636067 \ No newline at end of file diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs index 56cf6e4cd..32d20e0f0 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs @@ -79,15 +79,15 @@ public abstract class RGLObject : 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 { 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() } } } - } - /// - /// Some objects (such as NPC) use skinned meshes, which needs to be constantly updated by the Unity side. - /// - 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)); } } } diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs index d20d39f1a..7e3110ca8 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs +++ b/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,7 +55,10 @@ 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); @@ -60,6 +66,9 @@ public static class RGLNativeAPI [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) diff --git a/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs b/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs index 4174109a0..226febd32 100644 --- a/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs +++ b/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs @@ -98,7 +98,7 @@ public void Awake() public void Start() { - sceneManager = FindObjectOfType(); + sceneManager = SceneManager.Instance; if (sceneManager == null) { // TODO(prybicki): this is too tedious, implement automatic instantiation of RGL Scene Manager diff --git a/Assets/RGLUnityPlugin/Scripts/SceneManager.cs b/Assets/RGLUnityPlugin/Scripts/SceneManager.cs index ec8a04631..d78f65a95 100644 --- a/Assets/RGLUnityPlugin/Scripts/SceneManager.cs +++ b/Assets/RGLUnityPlugin/Scripts/SceneManager.cs @@ -46,6 +46,9 @@ public enum MeshSource RegularMeshesAndSkinnedMeshes } + // Singleton pattern + public static SceneManager Instance { get; private set; } + [SerializeField] private MeshSource meshSource = MeshSource.RegularMeshesAndSkinnedMeshes; @@ -66,8 +69,11 @@ public enum MeshSource // Since categoryId can be changed in the runtime, this is filled only on object removal / simulation end. private readonly Dictionary semanticDict = new Dictionary(); - private int lastUpdateFrame = -1; - private int lastFixedUpdateFrame = -1; + // Last frame of the simulation in which there was an update. + public int LastUpdateFrame { get; private set; } = -1; + + // Last FixedUpdate (physics cycle) in the `LastUpdateFrame` in which there was an update. + public int LastFixedUpdateFrame { get; private set; } = -1; private void OnDisable() { @@ -81,6 +87,14 @@ private void OnValidate() private void Awake() { + if (Instance != null && Instance != this) + { + Debug.LogError("SceneManager is already on the scene. Removing this component"); + Destroy(this); + return; + } + Instance = this; + if (IntoRGLObjects == null) { UpdateMeshSource(); @@ -108,8 +122,9 @@ private void UpdateMeshSource() /// /// Find out what changes happened on the scene since the last update and update RGL data. /// - /// Indicates fixed update frame number to enable updating when FixedUpdate is called more frequently than Update. - public void DoUpdate(int fixedUpdateFrame = 0) + /// Indicates which in turn the FixedUpdate call in the current frame it is. + /// It enables updating when FixedUpdate is called more frequently than Update. + public void DoUpdate(int fixedUpdateInCurrentFrame = 0) { /* TODO(prybicki): * Placing this code in Update() might cause an artifact - only undefined subset of @@ -127,13 +142,13 @@ public void DoUpdate(int fixedUpdateFrame = 0) // The problem was that lidars updated their position every raytrace execution (fixedUpdate) while RGL scene did not update because it was the same frame of the simulation. // In this case, we are raytracing on the same scene changing only lidars position which may overlap with the body of not-updated objects. // Now, lidars track its FixedUpdate cycles in the currect frame and pass it as fixedUpdateFrame. - if (lastUpdateFrame == Time.frameCount && lastFixedUpdateFrame == fixedUpdateFrame) + if (LastUpdateFrame == Time.frameCount && LastFixedUpdateFrame == fixedUpdateInCurrentFrame) { return; // Already done in this frame and fixed update (physics cycle) } - lastFixedUpdateFrame = fixedUpdateFrame; - lastUpdateFrame = Time.frameCount; + LastFixedUpdateFrame = fixedUpdateInCurrentFrame; + LastUpdateFrame = Time.frameCount; SynchronizeSceneTime(); @@ -210,6 +225,7 @@ private void Clear() RGLMeshSharingManager.Clear(); RGLTextureSharingManager.Clear(); + BonesPoseCacheManager.Clear(); uploadedRGLObjects.Clear(); lastFrameGameObjects.Clear(); Debug.Log("RGLSceneManager: cleared");