From 60d86a1dfc3da17c5479e59f5d9d652a5cb9274a Mon Sep 17 00:00:00 2001 From: Yogi <1273750265@qq.com> Date: Sun, 1 Dec 2024 17:25:54 +0800 Subject: [PATCH] update --- Assets/Scenes/MainScene.unity | 14 ++++---- Assets/Scripts/GameManager.cs | 66 ++++++++++++++++++++++++----------- 2 files changed, 53 insertions(+), 27 deletions(-) diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity index 70291ea..60f6ba1 100644 --- a/Assets/Scenes/MainScene.unity +++ b/Assets/Scenes/MainScene.unity @@ -674,7 +674,7 @@ PrefabInstance: m_Modifications: - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalPosition.x - value: 0.57 + value: 0.07 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalPosition.y @@ -682,23 +682,23 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalPosition.z - value: 0 + value: -0.04 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalRotation.w - value: 1 + value: 0.7071068 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalRotation.x - value: -0 + value: 0 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalRotation.y - value: -0 + value: 0.7071068 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalRotation.z - value: -0 + value: 0 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalEulerAnglesHint.x @@ -706,7 +706,7 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalEulerAnglesHint.y - value: 0 + value: 90 objectReference: {fileID: 0} - target: {fileID: -8679921383154817045, guid: 89b9ffd9832690d4ba423ff477aa1d06, type: 3} propertyPath: m_LocalEulerAnglesHint.z diff --git a/Assets/Scripts/GameManager.cs b/Assets/Scripts/GameManager.cs index 8088fea..aa5b133 100644 --- a/Assets/Scripts/GameManager.cs +++ b/Assets/Scripts/GameManager.cs @@ -1,4 +1,5 @@ using System.Text; +using System.Threading.Tasks; using UnityEngine; public class GameManager : MonoBehaviour @@ -26,6 +27,7 @@ public class GameManager : MonoBehaviour } private void OnReceivedMessage(string msgStr) { + if (msgStr == startMsg) { print($"开始游戏"); @@ -53,28 +55,11 @@ public class GameManager : MonoBehaviour var gz = float.Parse(arr[5]); print($"获得加速度:{ax},{ay},{az}"); print($"获得角速度:{gx},{gy},{gz}"); - + //this.GetComponent().angularVelocity if (isCanControlModel) { - // 时间间隔 - float dt = 0.01f; - - // 通过加速度计算姿态（俯仰角和滚转角） - float roll = Mathf.Atan2(ay, az) * Mathf.Rad2Deg; // 滚转角 - float pitch = Mathf.Atan2(-ax, Mathf.Sqrt(ay * ay + az * az)) * Mathf.Rad2Deg; // 俯仰角 - - // 积分得到角度变化 - angleX = gx * dt; // 绕X轴旋转 - angleY = gy * dt; // 绕Y轴旋转 - angleZ = gz * dt; // 绕Z轴旋转 - - // 使用互补滤波来平滑姿态估计 - // 这里的系数决定了加速度和陀螺仪的权重，通常设置为0.98 - float alpha = 0.98f; // 互补滤波的权重 - float filteredPitch = alpha * (angleX) + (1 - alpha) * pitch; // 过滤后的俯仰角 - float filteredRoll = alpha * (angleY) + (1 - alpha) * roll; // 过滤后的滚转角 - - ControlModelRotate(new Vector3(filteredRoll, filteredPitch, angleZ)); // 传入过滤后的值 + //ControlModelRotate(new Vector3(filteredRoll, filteredPitch, angleZ)); // 传入过滤后的值 + brainTrans.rotation = CalculateRotation(brainTrans.rotation, ax, ay, az, gx, gy, gz); } } } @@ -92,7 +77,26 @@ public class GameManager : MonoBehaviour { brainTrans.rotation = Quaternion.Euler(angleModify); } + private Quaternion CalculateRotation(Quaternion rotation, float ax, float ay, float az, float gx, float gy, float gz) + { + // 假设你已经有初始四元数 + Quaternion q = rotation; + // 将角速度转换为四元数 + Quaternion gyroQuat = new Quaternion(0, gx * dt, gy * dt, gz * dt); + + // 更新四元数：四元数乘积 + q = q * gyroQuat; + + // 归一化四元数，防止数值误差积累 + q.Normalize(); + + // 使用加速度计数据修正四元数 + Quaternion accelQuat = new Quaternion(ax, ay, az, 0); + q = Quaternion.Slerp(q, accelQuat, 0.01f); // 使用球形插值修正四元数 + // 最终得到物体的旋转 + return q; + } private StringBuilder sb; private void Update() @@ -128,6 +132,28 @@ public class GameManager : MonoBehaviour Debug.LogWarning($"录制结束! 参数:\n{sb}"); sb = null; } + if (Input.GetKeyDown(KeyCode.C)) + { + print(sendMsgDataArr); + SendAsync(); + } + } + + async void SendAsync() + { + var rows = sendMsgDataArr.Split("\r\n"); + int index = 0; + while (index < rows.Length) + { + var row = rows[index]; + OnReceivedMessage(row); + await Task.Delay(100); + index++; + } + } } + + + private string sendMsgDataArr = "0.03,0.00,0.83,-5.42,-3.05,-2.17\r\n0.07,-0.03,0.80,-4.53,-1.27,1.87\r\n0.04,-0.02,0.82,-7.34,-0.99,-1.26\r\n0.04,-0.02,0.82,-1.22,-3.95,3.66\r\n0.03,0.02,0.81,-6.32,-4.70,-3.34\r\n0.05,-0.03,0.82,-6.53,-2.40,-2.28\r\n0.05,-0.00,0.82,-6.65,-3.69,6.26\r\n0.04,-0.01,0.83,-4.22,-4.48,4.00\r\n0.07,-0.03,0.81,-5.82,-2.50,-5.01\r\n0.05,-0.01,0.82,-4.89,-2.97,-1.27\r\n0.04,0.01,0.82,-6.31,-2.71,0.60\r\n0.07,-0.02,0.81,-1.50,-1.15,-7.15\r\n0.04,0.01,0.82,-7.13,3.05,-15.97\r\n0.06,-0.03,0.81,-3.79,1.00,-45.29\r\n-0.03,-0.06,0.87,-6.42,0.47,-123.16\r\n-0.01,0.02,0.78,-10.50,4.44,-90.85\r\n0.06,-0.01,0.81,-6.31,7.85,-80.14\r\n0.05,-0.04,0.82,-14.08,-0.04,-110.42\r\n-0.08,-0.09,0.85,-11.08,-3.77,-86.89\r\n0.02,-0.01,0.82,-9.47,3.86,-72.44\r\n0.03,-0.05,0.82,-12.89,6.39,-133.51\r\n-0.00,-0.10,0.83,-27.43,2.75,-189.82\r\n0.03,-0.09,0.82,-14.63,7.84,-137.10\r\n0.09,-0.10,0.79,-19.21,9.53,-175.67\r\n0.02,-0.14,0.81,-13.94,3.36,-197.25\r\n0.03,-0.10,0.79,-23.36,2.68,-112.65\r\n0.03,-0.11,0.86,-9.88,12.50,-140.66\r\n0.01,-0.22,0.81,-12.49,12.49,-125.18\r\n0.09,-0.06,0.84,-9.07,11.02,-110.91\r\n-0.24,-0.09,0.77,-15.20,43.73,-75.92\r\n0.13,-0.10,0.80,-10.72,17.12,-129.89\r\n0.05,-0.14,0.80,-11.35,4.04,6.60\r\n0.03,-0.17,0.82,-5.33,-3.23,-9.43\r\n0.01,-0.18,0.79,-3.86,-2.73,0.50\r\n0.06,-0.12,0.85,-5.76,-0.42,-9.27\r\n0.07,-0.15,0.81,-4.90,-6.82,2.41\r\n0.04,-0.18,0.80,-3.12,-0.34,19.65\r\n0.06,-0.14,0.82,-4.18,-2.79,-7.72\r\n0.05,-0.14,0.80,-6.60,-2.23,-0.33"; }