diff --git a/Plugins/SprCSharp.dll b/Plugins/SprCSharp.dll index 759252e..08e303c 100644 Binary files a/Plugins/SprCSharp.dll and b/Plugins/SprCSharp.dll differ diff --git a/Plugins/SprCSharp.dll.meta b/Plugins/SprCSharp.dll.meta index 09bbd2b..d44c3cd 100644 --- a/Plugins/SprCSharp.dll.meta +++ b/Plugins/SprCSharp.dll.meta @@ -1,32 +1,33 @@ -fileFormatVersion: 2 -guid: d810f661eaaa93243952a18520db9f1f -timeCreated: 1518622597 -licenseType: Free -PluginImporter: - externalObjects: {} - serializedVersion: 2 - iconMap: {} - executionOrder: {} - isPreloaded: 0 - isOverridable: 0 - platformData: - - first: - Any: - second: - enabled: 1 - settings: {} - - first: - Editor: Editor - second: - enabled: 0 - settings: - DefaultValueInitialized: true - - first: - Windows Store Apps: WindowsStoreApps - second: - enabled: 0 - settings: - CPU: AnyCPU - userData: - assetBundleName: - assetBundleVariant: +fileFormatVersion: 2 +guid: d810f661eaaa93243952a18520db9f1f +PluginImporter: + externalObjects: {} + serializedVersion: 2 + iconMap: {} + executionOrder: {} + defineConstraints: [] + isPreloaded: 0 + isOverridable: 0 + isExplicitlyReferenced: 0 + validateReferences: 1 + platformData: + - first: + Any: + second: + enabled: 1 + settings: {} + - first: + Editor: Editor + second: + enabled: 0 + settings: + DefaultValueInitialized: true + - first: + Windows Store Apps: WindowsStoreApps + second: + enabled: 0 + settings: + CPU: AnyCPU + userData: + assetBundleName: + assetBundleVariant: diff --git a/Plugins/SprExport.dll b/Plugins/SprExport.dll index 3b286f9..0105b21 100644 Binary files a/Plugins/SprExport.dll and b/Plugins/SprExport.dll differ diff --git a/Plugins/SprImport.dll b/Plugins/SprImport.dll index 8e0738e..8f55218 100644 Binary files a/Plugins/SprImport.dll and b/Plugins/SprImport.dll differ diff --git a/Plugins/SprImport.dll.meta b/Plugins/SprImport.dll.meta index 2cc3b7e..05332e4 100644 --- a/Plugins/SprImport.dll.meta +++ b/Plugins/SprImport.dll.meta @@ -1,32 +1,33 @@ -fileFormatVersion: 2 -guid: e51df223c5ae5bc4a9c96d26531b5e84 -timeCreated: 1518622598 -licenseType: Free -PluginImporter: - externalObjects: {} - serializedVersion: 2 - iconMap: {} - executionOrder: {} - isPreloaded: 0 - isOverridable: 0 - platformData: - - first: - Any: - second: - enabled: 1 - settings: {} - - first: - Editor: Editor - second: - enabled: 0 - settings: - DefaultValueInitialized: true - - first: - Windows Store Apps: WindowsStoreApps - second: - enabled: 0 - settings: - CPU: AnyCPU - userData: - assetBundleName: - assetBundleVariant: +fileFormatVersion: 2 +guid: 103a6d161a4e05a4890bcd95871c06cf +PluginImporter: + externalObjects: {} + serializedVersion: 2 + iconMap: {} + executionOrder: {} + defineConstraints: [] + isPreloaded: 0 + isOverridable: 0 + isExplicitlyReferenced: 0 + validateReferences: 1 + platformData: + - first: + Any: + second: + enabled: 1 + settings: {} + - first: + Editor: Editor + second: + enabled: 0 + settings: + DefaultValueInitialized: true + - first: + Windows Store Apps: WindowsStoreApps + second: + enabled: 0 + settings: + CPU: AnyCPU + userData: + assetBundleName: + assetBundleVariant: diff --git a/Plugins/glui32.dll b/Plugins/glui32.dll index ebe49fe..84bc513 100644 Binary files a/Plugins/glui32.dll and b/Plugins/glui32.dll differ diff --git a/Plugins/sprTetgen.dll b/Plugins/sprTetgen.dll index a8f93dd..7de8131 100644 Binary files a/Plugins/sprTetgen.dll and b/Plugins/sprTetgen.dll differ diff --git a/Scripts/Collision/CDShapeBehaviour.cs b/Scripts/Collision/CDShapeBehaviour.cs index 5bc60ab..61e37ec 100644 --- a/Scripts/Collision/CDShapeBehaviour.cs +++ b/Scripts/Collision/CDShapeBehaviour.cs @@ -27,6 +27,7 @@ public override ObjectIf Build() { } CDShapeIf shape = CreateShape(shapeObject); + shape.SetName("cd:" + gameObject.name); phSolid.AddShape(shape); GameObject solidObject = solidBehaviour.gameObject; diff --git a/Scripts/Common/CoordinateTypeConversion.cs b/Scripts/Common/CoordinateTypeConversion.cs index 0e2b865..0632331 100644 --- a/Scripts/Common/CoordinateTypeConversion.cs +++ b/Scripts/Common/CoordinateTypeConversion.cs @@ -1,6 +1,7 @@ using UnityEngine; using System.Collections; using SprCs; +using System; namespace SprUnity { public static class SprUnityExtentions { @@ -9,7 +10,8 @@ public static class SprUnityExtentions { public static Posed ToPosed(this Transform t) { Vector3 p = t.position; Quaternion q = t.rotation; - return new Posed(q.w, q.x, q.y, q.z, p.x, p.y, p.z); + double norm = Math.Sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + return new Posed(q.w / norm, q.x / norm, q.y / norm, q.z / norm, p.x, p.y, p.z); } public static Vec3f ToVec3f(this Vector3 v) { @@ -25,12 +27,13 @@ public static Quaternionf ToQuaternionf(this Quaternion q) { } public static Quaterniond ToQuaterniond(this Quaternion q) { - return new Quaterniond(q.w, q.x, q.y, q.z); + double norm = Math.Sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + return new Quaterniond(q.w / norm, q.x / norm, q.y / norm, q.z / norm); } public static arraywrapper_double ToArrayWrapperDouble(this double[] d) { int l = d.Length; - arraywrapper_double wd = new arraywrapper_double(nelm: (uint)l); + arraywrapper_double wd = new arraywrapper_double(nelm: l); for (int i = 0; i < l; i++) { wd[i] = d[i]; } diff --git a/Scripts/Common/MeshRoundCone.cs b/Scripts/Common/MeshRoundCone.cs index 3384abb..360896f 100644 --- a/Scripts/Common/MeshRoundCone.cs +++ b/Scripts/Common/MeshRoundCone.cs @@ -167,7 +167,9 @@ public void CreateMesh() { } } - meshRenderer.sharedMaterial = default_material; + if (default_material != null && meshRenderer.sharedMaterial == null) { + meshRenderer.sharedMaterial = default_material; + } } public void Reshape() { diff --git a/Scripts/Common/SprBehaviour.cs b/Scripts/Common/SprBehaviour.cs index 35f8493..7137281 100644 --- a/Scripts/Common/SprBehaviour.cs +++ b/Scripts/Common/SprBehaviour.cs @@ -89,7 +89,7 @@ public static void ExecLateAwakeStart() { // 対応するSpringheadオブジェクト private ObjectIf sprObject_ = null; - public ObjectIf sprObject { get { return sprObject_; } protected set { sprObject_ = value; } } + public ObjectIf sprObject { get { return sprObject_; } protected set { sprObject_ = value; } } // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // 派生クラスで実装するメソッド @@ -119,12 +119,13 @@ public void Reset() { private bool awakeCalled = false; public virtual void Awake() { AwakeImpl(); } public void AwakeImpl(bool fromLateExecQueue = false) { + if(awakeCalled) { return; } if (lateAwakeStart && !(fromLateExecQueue)) { // 今すぐ実行せず、待ち行列に入れる lateAwakeQueue.Enqueue(this); } else { - if (!awakeCalled && GetDescStruct() != null) { + if (GetDescStruct() != null) { if (!enabled) { return; } sprObject = Build(); if (sprObject != null) { @@ -140,12 +141,13 @@ public void AwakeImpl(bool fromLateExecQueue = false) { private bool startCalled = false; public virtual void Start() { StartImpl(); } public virtual void StartImpl(bool fromLateExecQueue = false) { + if (startCalled) { return; } if (lateAwakeStart && !(fromLateExecQueue)) { // 今すぐ実行せず、待ち行列に入れる lateStartQueue.Enqueue(this); } else { - if (!startCalled && GetDescStruct() != null) { + if (GetDescStruct() != null) { Link(); // オブジェクトの作成が一通り完了したら一度OnValidateを読んで設定を確実に反映しておく OnValidate(); @@ -204,10 +206,10 @@ public PHSceneIf phScene { } } - // + // phScene.GetSdkを使用すると、FwAppを使用した場合nullになってしまうためPHSdkif.phSdkIfを使用 public PHSdkIf phSdk { get { - return phScene.GetSdk(); + return PHSdkIf.phSdkIf; } } } diff --git a/Scripts/HumanInterface/HISpidarBehaviour.cs b/Scripts/HumanInterface/HISpidarBehaviour.cs index 836c789..e120f6b 100644 --- a/Scripts/HumanInterface/HISpidarBehaviour.cs +++ b/Scripts/HumanInterface/HISpidarBehaviour.cs @@ -81,7 +81,7 @@ void FixedUpdate() { if (lengthText != null) { string text = ""; - for (int i = 0; i < hiSpidar.NMotor(); i++) { + for (int i = 0; i < (int)hiSpidar.NMotor(); i++) { text += hiSpidar.GetMotor((uint)i).GetLength().ToString() + "\r\n"; } lengthText.text = text; diff --git a/Scripts/HumanInterface/HISpidarGBehaviour.cs b/Scripts/HumanInterface/HISpidarGBehaviour.cs index 28803cf..ff3bc31 100644 --- a/Scripts/HumanInterface/HISpidarGBehaviour.cs +++ b/Scripts/HumanInterface/HISpidarGBehaviour.cs @@ -73,7 +73,7 @@ void FixedUpdate() { if (lengthText != null) { string text = ""; - for (int i = 0; i < hiSpidar.NMotor(); i++) { + for (int i = 0; i < (int)hiSpidar.NMotor(); i++) { text += hiSpidar.GetMotor((uint)i).GetLength().ToString() + "\r\n"; } lengthText.text = text; diff --git a/Scripts/Physics/PHBallJointBehaviour.cs b/Scripts/Physics/PHBallJointBehaviour.cs index 45b0b3f..8c19e19 100644 --- a/Scripts/Physics/PHBallJointBehaviour.cs +++ b/Scripts/Physics/PHBallJointBehaviour.cs @@ -10,8 +10,30 @@ [CustomEditor(typeof(PHBallJointBehaviour))] [CanEditMultipleObjects] public class PHBallJointBehaviourEditor : PHJointBehaviourEditor { + PHBallJointDesc desc = new PHBallJointDesc(); + public void OnSceneGUI() { base.OnSceneGUI(); + + PHBallJointBehaviour phBallJointBehaviour = target as PHBallJointBehaviour; + + if (phBallJointBehaviour.showJointTargetPositionHandle) { + Tools.current = Tool.None; + + if (phBallJointBehaviour.phBallJoint != null) { + phBallJointBehaviour.phBallJoint.GetDesc(desc); + Quaternion currTargetPosition = desc.targetPosition.ToQuaternion(); + Quaternion handleRot = Handles.RotationHandle(currTargetPosition, phBallJointBehaviour.transform.position); + desc.targetPosition = handleRot.ToQuaterniond(); + phBallJointBehaviour.phBallJoint.SetDesc(desc); + phBallJointBehaviour.desc.targetPosition = desc.targetPosition; + + } else if (phBallJointBehaviour.desc != null) { + Quaternion currPullbackTarget = ((Quaterniond)(phBallJointBehaviour.desc.targetPosition)).ToQuaternion(); + Quaternion handleRot = Handles.RotationHandle(currPullbackTarget, phBallJointBehaviour.transform.position); + phBallJointBehaviour.desc.targetPosition = handleRot.ToQuaterniond(); + } + } } } diff --git a/Scripts/Physics/PHFixJointBehavior.cs b/Scripts/Physics/PHFixJointBehavior.cs new file mode 100644 index 0000000..004d2fc --- /dev/null +++ b/Scripts/Physics/PHFixJointBehavior.cs @@ -0,0 +1,57 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using SprCs; + +#if UNITY_EDITOR +using UnityEditor; + +[CustomEditor(typeof(PHFixJointBehavior))] +[CanEditMultipleObjects] +public class PHFixJointBehaviorEditor : Editor { +} + +#endif +[DefaultExecutionOrder(4)] +public class PHFixJointBehavior : PHJointBehaviour { + // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- + // メンバ変数 + + public PHFixJointDescStruct desc = null; + + // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- + // このBehaviourに対応するSpringheadオブジェクト + + public PHFixJointIf phFixJoint { get { return sprObject as PHFixJointIf; } } + + // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- + // SprBehaviourの派生クラスで実装するメソッド + + // -- DescStructオブジェクトを再構築する + public override void ResetDescStruct() { + desc = new PHFixJointDescStruct(); + } + + // -- DescStructオブジェクトを取得する + public override CsObject GetDescStruct() { + return desc; + } + + // -- DescオブジェクトをNewして返す + public override CsObject CreateDesc() { + return new PHFixJointDesc(); + } + + // -- DescStructをDescに適用する + public override void ApplyDesc(CsObject from, CsObject to) { + (from as PHFixJointDescStruct).ApplyTo(to as PHFixJointDesc); + } + + // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- + // PHJointBehaviourの派生クラスで実装するメソッド + + // -- 関節を作成する + public override PHJointIf CreateJoint(PHSolidIf soSock, PHSolidIf soPlug) { + return phScene.CreateJoint(soSock, soPlug, PHFixJointIf.GetIfInfoStatic(), (PHFixJointDesc)desc); + } +} diff --git a/Scripts/Physics/PHFixJointBehavior.cs.meta b/Scripts/Physics/PHFixJointBehavior.cs.meta new file mode 100644 index 0000000..c7ced60 --- /dev/null +++ b/Scripts/Physics/PHFixJointBehavior.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dc4317adc5ff874439c607dda2b916b3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Scripts/Physics/PHHingeJointBehaviour.cs b/Scripts/Physics/PHHingeJointBehaviour.cs index a48e625..2f804ac 100644 --- a/Scripts/Physics/PHHingeJointBehaviour.cs +++ b/Scripts/Physics/PHHingeJointBehaviour.cs @@ -8,9 +8,33 @@ using UnityEditor; [CustomEditor(typeof(PHHingeJointBehaviour))] +[CanEditMultipleObjects] public class PHHingeJointBehaviourEditor : PHJointBehaviourEditor { + PHHingeJointDesc desc = new PHHingeJointDesc(); + public void OnSceneGUI() { base.OnSceneGUI(); + + PHHingeJointBehaviour phHingeJointBehaviour = target as PHHingeJointBehaviour; + + if (phHingeJointBehaviour.showJointTargetPositionHandle) { + Tools.current = Tool.None; + /* + if (phHingeJointBehaviour.phHingeJoint != null) { + phHingeJointBehaviour.phHingeJoint.GetDesc(desc); + double currTargetPosition = desc.targetPosition; + Quaternion handleRot = Handles.Di(currTargetPosition, phBallJointBehaviour.transform.position); + desc.targetPosition = handleRot.ToQuaterniond(); + phBallJointBehaviour.phBallJoint.SetDesc(desc); + phBallJointBehaviour.desc.targetPosition = desc.targetPosition; + + } else if (phHingeJointBehaviour.desc != null) { + Quaternion currPullbackTarget = ((Quaterniond)(phBallJointBehaviour.desc.targetPosition)).ToQuaternion(); + Quaternion handleRot = Handles.RotationHandle(currPullbackTarget, phBallJointBehaviour.transform.position); + phBallJointBehaviour.desc.targetPosition = handleRot.ToQuaterniond(); + } + */ + } } } diff --git a/Scripts/Physics/PHJointBehaviour.cs b/Scripts/Physics/PHJointBehaviour.cs index 2e87452..ce4ec07 100644 --- a/Scripts/Physics/PHJointBehaviour.cs +++ b/Scripts/Physics/PHJointBehaviour.cs @@ -12,7 +12,7 @@ public void OnSceneGUI() { PHJointBehaviour phJointBehaviour = (PHJointBehaviour)target; // ----- ----- ----- ----- ----- - // Target Position Handle + // Joint Pose Handle if (phJointBehaviour.showJointPoseHandle) { Tools.current = Tool.None; // この方式だと微小変化が常に発生してあまりUndoなどが機能しない @@ -41,10 +41,12 @@ public abstract class PHJointBehaviour : SprSceneObjBehaviour { public GameObject plug = null; public GameObject jointObject = null; + // Joint自体の位置姿勢の描画・編集有効化 public bool showJointPoseHandle; + // Jointの目標角度の描画・編集有効化(Jointの種類によって描画変化) public bool showJointTargetPositionHandle; public Vector3 jointPosition = new Vector3(); - public Quaternion jointOrientation =Quaternion.identity; + public Quaternion jointOrientation = Quaternion.identity; // 関節で接続された剛体同士の衝突を無効ににするかどうか public bool disableCollision = false; @@ -65,21 +67,41 @@ public abstract class PHJointBehaviour : SprSceneObjBehaviour { // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // SprBehaviourの派生クラスで実装するメソッド + public Posed socketPose; + public Posed plugPose; + // -- Sprオブジェクトの構築を行う public override ObjectIf Build() { if (!socket) { socket = gameObject.transform.parent.GetComponentInParent().gameObject; } - if (!plug) { plug = gameObject.GetComponentInParent().gameObject; } + if (!plug) { plug = gameObject.GetComponentInParent().gameObject; } if (socket == null) { throw new ObjectNotFoundException("Socket object did not found for Joint", gameObject); } if (plug == null) { throw new ObjectNotFoundException("Plug object did not found for Joint", gameObject); } PHSolidIf soSock = socket.GetComponent().sprObject as PHSolidIf; - PHSolidIf soPlug = plug.GetComponent().sprObject as PHSolidIf; + PHSolidIf soPlug = plug.GetComponent().sprObject as PHSolidIf; + + // SocketとPlugを設定。SetSocketPose/SetPlugPoseを使わずdescの段階でセット。 + if (autoSetSockPlugPose) { + Posed jointPose = new Posed(); + if (jointObject == null) { + jointObject = gameObject; + jointPose = jointObject.transform.ToPosed() * new Posed(jointPosition.ToVec3d(), jointOrientation.ToQuaterniond()); + } else { + jointPose = jointObject.transform.ToPosed(); + } + var desc = (PHConstraintDescStruct)GetDescStruct(); + desc.poseSocket = soSock.GetPose().Inv() * jointPose; + desc.posePlug = soPlug.GetPose().Inv() * jointPose; + } PHJointIf jo = CreateJoint(soSock, soPlug); jo.SetName("jo:" + gameObject.name); + // SetSocketPose / SetPlugPoseが動かなくなているかもしれない。 (2022/01) + // 以前はこの段階でSocketとPlugを設定していたが、上でdescにセットするようにした。 + /* if (autoSetSockPlugPose) { // priority jointObject > jointPosition/Orientation > gameObject Posed jointPose = new Posed(); @@ -89,9 +111,17 @@ public override ObjectIf Build() { } else { jointPose = jointObject.transform.ToPosed(); } - jo.SetSocketPose(soSock.GetPose().Inv() * jointPose); - jo.SetPlugPose(soPlug.GetPose().Inv() * jointPose); + Posed socketPose = soSock.GetPose().Inv() * jointPose; + Posed plugPose = soPlug.GetPose().Inv() * jointPose; + jo.SetSocketPose(socketPose); + jo.SetPlugPose(plugPose); + + // 後でDescStructを元にSetDescするため、DescStructの更新が必要 + PHJointDescStruct jointDescStruct = GetDescStruct() as PHJointDescStruct; + jointDescStruct.poseSocket = socketPose; + jointDescStruct.posePlug = plugPose; } + */ return jo; } diff --git a/Scripts/Physics/PHOpBehaviour.cs b/Scripts/Physics/PHOpBehaviour.cs index 5d34705..0c1fcdf 100644 --- a/Scripts/Physics/PHOpBehaviour.cs +++ b/Scripts/Physics/PHOpBehaviour.cs @@ -244,7 +244,8 @@ void FixedUpdate () { //Marshal Copy for vertices, because objTargetVtsArr is slow try{ - Marshal.Copy(opObjAddr.objTargetVtsArr._this, dataArray, 0, copySize); + //Marshal.Copy(opObjAddr.objTargetVtsArr._thisArray[0], dataArray, 0, copySize); + Marshal.Copy(opObjAddr.objTargetVtsArr._this, dataArray, 0, copySize); // UύX } catch (ArgumentOutOfRangeException e){ Console.WriteLine(e.Message); diff --git a/Scripts/Physics/PHRootNodeBehaviour.cs b/Scripts/Physics/PHRootNodeBehaviour.cs index 3840dcf..e66d942 100644 --- a/Scripts/Physics/PHRootNodeBehaviour.cs +++ b/Scripts/Physics/PHRootNodeBehaviour.cs @@ -7,6 +7,7 @@ using SprCs; using SprUnity; +[DefaultExecutionOrder(5)] public class PHRootNodeBehaviour : SprSceneObjBehaviour { // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // メンバ変数 @@ -41,17 +42,41 @@ public override void ApplyDesc(CsObject from, CsObject to) { } // -- Sprオブジェクトの構築を行う - public override ObjectIf Build(){ - return null; + public override ObjectIf Build() { + var solid = this.GetComponent().phSolid; + sprObject = phScene.CreateRootNode(solid, desc); + CreateTreeNodesRecurs(phRootNode, solid); + phRootNode.Enable(); + phRootNode.SetName("RootNode:" + gameObject.name); + return phRootNode; } // -- 全てのBuildが完了した後に行う処理を書く。オブジェクト同士をリンクするなど public override void Link() { - if (sprObject == null) { - sprObject = phScene.CreateRootNode(this.GetComponent().phSolid); - Debug.Log("CreateRootNode[" + this.name + "]"); - phRootNode.Enable(); - } } + // RootNodeからJointのPlugを辿り、全てのTreeNodeを再帰的に作成 + private void CreateTreeNodesRecurs(PHTreeNodeIf node, PHSolidIf solid) { + var allTreeNodes = Resources.FindObjectsOfTypeAll().Where(n => n.gameObject.activeInHierarchy); + for (int i = 0; i < phScene.NJoints(); i++) { + var joint = phScene.GetJoint(i); + // PHJointBehaviourがアタッチされたGameObjectにPHTreeNodeBehaviourがアタッチされているかを確認 + var treeNodeBehaviours = allTreeNodes.Where(n => n.gameObject.activeInHierarchy && n.GetComponent()?.phJoint == joint); + if (treeNodeBehaviours.Count() == 0) continue; + else if (treeNodeBehaviours.Count() > 1) { + Debug.LogError("一つのJointに対して複数のPHTreeNodeがアタッチされています"); + } + var treeNodeBehaviour = treeNodeBehaviours.First(); + var socket = joint.GetSocketSolid(); + var plug = joint.GetPlugSolid(); + if (socket == solid && plug.GetTreeNode() == null) { + PHTreeNodeIf childNode = phScene.CreateTreeNode(node, plug); + if (childNode != null) { // JointがTreeNodeをサポートしていない場合がある + //Debug.Log("CreateTreeNode[" + this.name + "] = Tree(" + node.GetName() + ") <= Solid(" + plug.GetName() + ")"); + treeNodeBehaviour.SetTreeNode(childNode); + CreateTreeNodesRecurs(childNode, plug); + } + } + } + } } diff --git a/Scripts/Physics/PHSceneBehaviour.cs b/Scripts/Physics/PHSceneBehaviour.cs index 773e030..76685cb 100644 --- a/Scripts/Physics/PHSceneBehaviour.cs +++ b/Scripts/Physics/PHSceneBehaviour.cs @@ -4,6 +4,7 @@ using System.Runtime.InteropServices; using System; using System.Linq; +using System.Threading; #if UNITY_EDITOR using UnityEditor; @@ -125,8 +126,8 @@ public class PHSceneBehaviour : SprBehaviour { protected List phSolidBehaviours = new List(); protected List phIKEndEffectorBehaviours = new List(); - private static PHSdkIf phSdk = null; - protected static FWApp fwApp = null; + public static PHSdkIf phSdk = null; + public static FWApp fwApp = null; public PHSceneDescStruct desc = null; public PHIKEngineDescStruct descIK = null; @@ -137,6 +138,8 @@ public class PHSceneBehaviour : SprBehaviour { public bool enableDebugWindow = false; + public bool multiThreadMode = false; + [Serializable] public class CollisionSetting { public enum CollisionTargetSettingMode { All, One, NameMatching }; @@ -157,11 +160,13 @@ public enum CollisionTargetSettingMode { All, One, NameMatching }; // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // DefaultExecutionOrderでは不十分なためPHSceneのFixedUpdateで特定の順番で呼べるようにする public enum CallbackPriority { - BeforeStep, + BeforeUpdateSolidFromGameObject, + BeforeStep, + BeforeUpdateGameObjectFromSolid, Finally } public delegate void PHSceneBehaviourCallback(); - private class PHSceneBehaviourCallbackItem { + protected class PHSceneBehaviourCallbackItem { public int subPriority; public PHSceneBehaviourCallback callback; public PHSceneBehaviourCallbackItem(int subPriority, PHSceneBehaviourCallback callback) { @@ -169,8 +174,10 @@ public PHSceneBehaviourCallbackItem(int subPriority, PHSceneBehaviourCallback ca this.callback = callback; } } + // 優先度付きコールバックのリスト。Add/Removeの際にpriorityに従ってsortする - private Dictionary> fixedUpdateCallbacks; + protected Dictionary> fixedUpdateCallbacks; + // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // このBehaviourに対応するSpringheadオブジェクト @@ -216,6 +223,7 @@ public override ObjectIf Build() { while (fwApp.GetSdk() == null || fwApp.GetSdk().GetPHSdk() == null) { System.Threading.Thread.Sleep(10); } phSdk = fwApp.GetSdk().GetPHSdk(); + PHSdkIf.phSdkIf = phSdk; phScene = fwApp.GetSdk().GetScene(0).GetPHScene(); phScene.Clear(); phScene.SetDesc((PHSceneDesc)desc); @@ -223,11 +231,21 @@ public override ObjectIf Build() { FWSceneIf fwSceneIf = fwApp.GetSdk().GetScene(0); fwSceneIf.EnableRenderContact(true); fwSceneIf.EnableRenderForce(false, true); + phScene.multiThreadMode = multiThreadMode; //fwSceneIf.SetForceScale(0.01f, 0.01f); } else { phSdk = PHSdkIf.CreateSdk(); phScene = phSdk.CreateScene((PHSceneDesc)desc); + phScene.multiThreadMode = multiThreadMode; + + PHConstraintEngineDesc constraintEngineDesc = new PHConstraintEngineDesc(); + phScene.GetConstraintEngine().GetDesc(constraintEngineDesc); + //constraintEngineDesc.velCorrectionRate = 0.3; + //constraintEngineDesc.shrinkRate = 0.95; + Debug.Log("constraintEngineDesc.velCorrectionRate " + constraintEngineDesc.velCorrectionRate); + Debug.Log("constraintEngineDesc.shirinkRate " + constraintEngineDesc.shrinkRate); + phScene.GetConstraintEngine().SetDesc(constraintEngineDesc); } return phScene; @@ -235,7 +253,7 @@ public override ObjectIf Build() { // -- 全てのBuildが完了した後に行う処理を書く。オブジェクト同士をリンクするなど public override void Link() { - OnValidate(); + OnValidate(); } // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- @@ -262,18 +280,14 @@ public override void Start() { } void FixedUpdate() { if (enableUpdate) { - //lock (sprObject) { + foreach (var callBackItem in fixedUpdateCallbacks[CallbackPriority.BeforeUpdateSolidFromGameObject]) { + callBackItem.callback(); + } foreach (var phSolidBehaviour in phSolidBehaviours) { if (phSolidBehaviour != null) { - phSolidBehaviour.UpdatePose(); + phSolidBehaviour.UpdateSolidFromGameObject(); } - } - //} - if (fwApp != null) { - fwApp.PostRedisplay(); - } - } - if (sprObject != null && enableStep) { + } foreach (var phSolidBehaviour in phSolidBehaviours) { phSolidBehaviour.BeforeStep(); } @@ -285,9 +299,23 @@ void FixedUpdate() { foreach (var callBackItem in fixedUpdateCallbacks[CallbackPriority.BeforeStep]) { callBackItem.callback(); } - //lock (sprObject) { + if (sprObject != null && enableStep) { (sprObject as PHSceneIf).Step(); - //} + } + foreach (var callBackItem in fixedUpdateCallbacks[CallbackPriority.BeforeUpdateGameObjectFromSolid]) { + callBackItem.callback(); + } + foreach (var phSolidBehaviour in phSolidBehaviours) { + if (phSolidBehaviour != null) { + phSolidBehaviour.UpdateGameObjectFromSolid(); + } + } + foreach (var callBackItem in fixedUpdateCallbacks[CallbackPriority.Finally]) { + callBackItem.callback(); + } + if (fwApp != null) { + fwApp.PostRedisplay(); + } } } @@ -329,11 +357,11 @@ private void OnDestroy() { if (fwApp != null) { fwApp.EndThread(); fwApp = null; - if (phSdk != null) { - phSdk.Clear(); - phSdk = null; - } } + if (phSdk != null) { + phSdk.Clear(); + phSdk = null; + } } @@ -414,7 +442,7 @@ public void DeleteFixedUpadateCallback(PHSceneBehaviourCallback phSceneBehaviour callBackItems.Remove(deleteCallBackItem); } - public void RegisterPHSolidBehaviour(PHSolidBehaviour phSolid) { + public virtual void RegisterPHSolidBehaviour(PHSolidBehaviour phSolid) { phSolidBehaviours.Add(phSolid); // スキンメッシュ描画時のカクつきを防ぐため、ツリー深さでソートしておく。 @@ -424,4 +452,4 @@ public void RegisterPHSolidBehaviour(PHSolidBehaviour phSolid) { public void RegisterPHIKEndEffectorBehaviour(PHIKEndEffectorBehaviour phIKEndEffector) { phIKEndEffectorBehaviours.Add(phIKEndEffector); } -} +} diff --git a/Scripts/Physics/PHSolidBehaviour.cs b/Scripts/Physics/PHSolidBehaviour.cs index e70aacc..fc1432f 100644 --- a/Scripts/Physics/PHSolidBehaviour.cs +++ b/Scripts/Physics/PHSolidBehaviour.cs @@ -191,7 +191,54 @@ public override void OnValidate() { public void BeforeStep() { UpdateCenterOfMass(); } + // GameObjectの位置姿勢をSpringhead剛体に適用 適用対象(fixedSolid,dynamicalOff)の親にSolidがあると問題が起こる + public void UpdateSolidFromGameObject() { + if (sprObject != null) { + PHSolidIf so = sprObject as PHSolidIf; + if (fixedSolid) { + // Fixedな剛体はHandleの位置をSpringheadに反映 + //so.SetPose(new Posed(fixedSolidPosition.ToVec3d(), fixedSolidRotation.ToQuaterniond())); + //gameObject.transform.position = fixedSolidPosition; + //gameObject.transform.rotation = fixedSolidRotation; + } else { + // Fixedでない剛体の場合 + if (!so.IsDynamical()) { + // Dynamicalでない剛体はUnityの位置をSpringheadに反映（操作可能） + //Debug.Log(name + " so.GetVelocity() " + so.GetVelocity()); + //Debug.Log(name + " so.GetAngurlarVel() " + so.GetAngularVelocity()); + //Debug.Log(name + " so.GetPose() " + so.GetPose()); + //so.SetVelocity((gameObject.transform.position.ToVec3d() - so.GetPose().Pos()) / phScene.GetTimeStep()); + //Debug.Log((gameObject.transform.position.ToVec3d() - so.GetPose().Pos()) / phScene.GetTimeStep()); + //so.SetPose(gameObject.transform.ToPosed()); + //gameObject.transform.FromPosed(so.GetPose()); + } + fixedSolidPosition = gameObject.transform.position; + fixedSolidRotation = gameObject.transform.rotation; + } + } + } + // Springhead剛体の位置姿勢をGameObjectに適用：更新順を制御するためPHSceneからまとめて呼び出す + public void UpdateGameObjectFromSolid() { + if (sprObject != null) { + PHSolidIf so = sprObject as PHSolidIf; + if (fixedSolid) { + // 親が動くと子のGameObjectは動くため修正 + //gameObject.transform.position = fixedSolidPosition; + //gameObject.transform.rotation = fixedSolidRotation; + } else { + // Fixedでない剛体の場合 + if (so.IsDynamical()) { + gameObject.transform.FromPosed(so.GetPose()); + //Debug.Log(so.GetName() + so.GetPose()); + var q = so.GetPose(); + //Debug.Log("test " + name + "(" + q.w + "," + q.x + "," + q.y + "," + q.z + ")"); + fixedSolidPosition = gameObject.transform.position; + fixedSolidRotation = gameObject.transform.rotation; + } + } + } + } // Springhead剛体とGameObjectの間での位置姿勢の同期：　更新順を制御するためPHSceneからまとめて呼び出す public void UpdatePose() { if (sprObject != null) { diff --git a/Scripts/Physics/PHSpringBehavior.cs b/Scripts/Physics/PHSpringBehavior.cs index 2a124fd..a28c809 100644 --- a/Scripts/Physics/PHSpringBehavior.cs +++ b/Scripts/Physics/PHSpringBehavior.cs @@ -59,5 +59,4 @@ public override void ApplyDesc(CsObject from, CsObject to) { public override PHJointIf CreateJoint(PHSolidIf soSock, PHSolidIf soPlug) { return phScene.CreateJoint(soSock, soPlug, PHSpringIf.GetIfInfoStatic(), (PHSpringDesc)desc); } - } diff --git a/Scripts/Physics/PHTreeNodeBehaviour.cs b/Scripts/Physics/PHTreeNodeBehaviour.cs index f24c99a..e4787ca 100644 --- a/Scripts/Physics/PHTreeNodeBehaviour.cs +++ b/Scripts/Physics/PHTreeNodeBehaviour.cs @@ -7,6 +7,7 @@ using SprCs; using SprUnity; +// 対応するPHJointBehaviourの派生クラスがアタッチされているGameObjectにアタッチ public class PHTreeNodeBehaviour : SprSceneObjBehaviour { // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // メンバ変数 @@ -46,75 +47,12 @@ public override ObjectIf Build(){ } // -- 全てのBuildが完了した後に行う処理を書く。オブジェクト同士をリンクするなど + // PHRootNodeBehaviourで作成される public override void Link() { - // まず、このTreeNodeに付随する関節を探す。同じGameObjectに付いているはず - PHJointBehaviour jo = gameObject.GetComponent(); - - if (jo != null && jo.sprObject != null) { - // 次に、関節の親関節を探す。関節のソケット剛体を探し、それを基準に探す - // （親関節　＝　この関節のソケット剛体をプラグ剛体として持つ関節） - PHJointBehaviour joParent = null; - var jos = jo.socket.GetComponentsInChildren(); - foreach (var j in jos) { - if (j.plug == jo.socket) { joParent = j; break; } - } - - PHTreeNodeBehaviour parentTreeNode = null; - PHRootNodeBehaviour parentRootNode = null; - - if (joParent != null && jo != joParent) { - // -- 親関節があった場合 - parentTreeNode = joParent.GetComponent(); - parentRootNode = joParent.GetComponent(); - - if (parentTreeNode != null && parentTreeNode.isActiveAndEnabled) { - // 親ノードを先に構築する - if (parentTreeNode.sprObject == null) { parentTreeNode.Link(); } - - // その後、親TreeNodeに接続する形で自分のTreeNodeを作成する - if (sprObject == null) { - sprObject = phScene.CreateTreeNode(parentTreeNode.phTreeNode, - jo.plug.GetComponent().phSolid); - Debug.Log("CreateTreeNode[" + this.name + "] = Tree(" + parentTreeNode.name + ") <= Solid(" + jo.plug.name + ")"); - - phTreeNode.Enable(); - } - - } else if (parentRootNode != null && parentRootNode.isActiveAndEnabled) { - // 親ノードを先に構築する - if (parentRootNode.sprObject == null) { parentRootNode.Link(); } - - // その後、親RootNodeに接続する形で自分のTreeNodeを作成する - if (sprObject == null) { - sprObject = phScene.CreateTreeNode(parentRootNode.phRootNode, - jo.plug.GetComponent().phSolid); - Debug.Log("CreateTreeNode[" + this.name + "] = Root(" + parentRootNode.name + ") <= Solid(" + jo.plug.name + ")"); - - phTreeNode.Enable(); - } - - } else { - Debug.LogError("No Parent Node for " + this.name); - } - - } else { - // -- 親関節がない場合：この関節のsocketにRootNodeがくっついているはず - parentRootNode = jo.socket.GetComponent(); - - // 親ノードを先に構築する - if (parentRootNode.sprObject == null) { parentRootNode.Link(); } - - // その後、親RootNodeに接続する形で自分のTreeNodeを作成する - if (sprObject == null) { - sprObject = phScene.CreateTreeNode(parentRootNode.phRootNode, - jo.plug.GetComponent().phSolid); - Debug.Log("CreateTreeNode[" + this.name + "] = Root(" + parentRootNode.name + ") <= Solid(" + jo.plug.name + ")"); - - phTreeNode.Enable(); - } - - } - } } + // sprObjectのSetter、PHTreeNodeIfはPHRootNodeBehaviourで作成される + public void SetTreeNode(PHTreeNodeIf treeNode) { + sprObject = treeNode; + } }