ElbowKneeIKCtrl.cs 5.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173
  1. using System;
  2. using RootMotion.FinalIK;
  3. using UnityEngine;
  4. [Serializable]
  5. public class ElbowKneeIKCtrl : ALimbIKCtrl
  6. {
  7. public ElbowKneeIKCtrl(FBIKChain chain, IKMappingLimb ik_mapping, FullBodyIKMgr ik_ctrl, FullBodyIKMgr.IKEffectorType effector_type) : base(ik_ctrl, chain, ik_mapping, effector_type)
  8. {
  9. this.chain.bendConstraint.bendGoal = base.constraintTarget;
  10. base.isUpperBody = (effector_type == FullBodyIKMgr.IKEffectorType.Forearm_L || effector_type == FullBodyIKMgr.IKEffectorType.Forearm_R);
  11. this.FABRIK = base.constraintTarget.parent.gameObject.AddComponent<FABRIK>();
  12. this.FABRIK.solver.SetChain(new Transform[]
  13. {
  14. base.chainBones[0],
  15. base.chainBones[1]
  16. }, base.chainBones[0]);
  17. this.FABRIK.solver.target = base.constraintTarget;
  18. this.FABRIK.enabled = false;
  19. if (!base.isUpperBody)
  20. {
  21. if (this.isLeft)
  22. {
  23. base.limbCapsule = this.myIKMgr.body.limbColliderMgr.GetCollider(LimbColliderMgr.LimbType.Thigh_L);
  24. }
  25. else
  26. {
  27. base.limbCapsule = this.myIKMgr.body.limbColliderMgr.GetCollider(LimbColliderMgr.LimbType.Thigh_R);
  28. }
  29. }
  30. else if (this.isLeft)
  31. {
  32. base.limbCapsule = this.myIKMgr.body.limbColliderMgr.GetCollider(LimbColliderMgr.LimbType.UpperArm_L);
  33. }
  34. else
  35. {
  36. base.limbCapsule = this.myIKMgr.body.limbColliderMgr.GetCollider(LimbColliderMgr.LimbType.UpperArm_R);
  37. }
  38. }
  39. public override bool isLeft
  40. {
  41. get
  42. {
  43. return this.effectorType == FullBodyIKMgr.IKEffectorType.Forearm_L || this.effectorType == FullBodyIKMgr.IKEffectorType.Calf_L;
  44. }
  45. }
  46. public override ALimbIKCtrl pairIK
  47. {
  48. get
  49. {
  50. FullBodyIKMgr.IKEffectorType effector_type;
  51. if (base.isUpperBody)
  52. {
  53. effector_type = ((!this.isLeft) ? FullBodyIKMgr.IKEffectorType.Forearm_L : FullBodyIKMgr.IKEffectorType.Forearm_R);
  54. }
  55. else
  56. {
  57. effector_type = ((!this.isLeft) ? FullBodyIKMgr.IKEffectorType.Calf_L : FullBodyIKMgr.IKEffectorType.Calf_R);
  58. }
  59. return this.myIKMgr.GetIKCtrl<ALimbIKCtrl>(effector_type);
  60. }
  61. }
  62. public ShoulderThighIKCtrl shoulderThighCtrl { get; private set; }
  63. public HandFootIKCtrl handFootCtrl { get; private set; }
  64. public void SetChain(HandFootIKCtrl handfoot_data, ShoulderThighIKCtrl shoulderthigh_data)
  65. {
  66. this.handFootCtrl = handfoot_data;
  67. this.ChainChildCtrl = handfoot_data;
  68. this.shoulderThighCtrl = shoulderthigh_data;
  69. this.ChainParentCtrl = shoulderthigh_data;
  70. }
  71. public override void TargetTransCpy()
  72. {
  73. base.TargetTransCpy();
  74. this.chain.bendConstraint.weight = 0f;
  75. this.FABRIK.solver.IKPositionWeight = 0f;
  76. }
  77. public override void ApplyIKSetting()
  78. {
  79. this.BoneLocalRot = base.bone.localRotation;
  80. this.HandFootLocalRot = this.handFootCtrl.bone.localRotation;
  81. base.ApplyIKSetting();
  82. }
  83. protected override void SetTargetTransform(AIKCtrl.IKSettingData data, Vector3 pos, Quaternion rot)
  84. {
  85. base.SetTargetTransform(data, pos, rot);
  86. Vector3 vector = this.shoulderThighCtrl.bone.InverseTransformPoint(base.bone.position);
  87. Transform constraintTarget = this.shoulderThighCtrl.constraintTarget;
  88. if (data.isPointAttach)
  89. {
  90. constraintTarget.rotation = Quaternion.FromToRotation(base.bone.position - constraintTarget.position, base.constraintTarget.position - constraintTarget.position) * constraintTarget.rotation;
  91. if (!this.shoulderThighCtrl.pointIKData.isIKExecNotWeight0)
  92. {
  93. this.shoulderThighCtrl.positionWeight = Mathf.Max(base.positionWeight, this.shoulderThighCtrl.positionWeight);
  94. Vector3 position = base.bone.InverseTransformPoint(this.shoulderThighCtrl.bone.position);
  95. constraintTarget.position = base.constraintTarget.TransformPoint(position);
  96. }
  97. }
  98. else
  99. {
  100. Vector3 axis = base.constraintTarget.position - constraintTarget.position;
  101. Quaternion rotation = base.constraintTarget.rotation;
  102. Vector3 lhs = rotation * Vector3.forward;
  103. Vector3 rhs = base.bone.rotation * Vector3.forward;
  104. float f = Vector3.Dot(lhs, rhs);
  105. float num = Mathf.Acos(f) * 57.29578f;
  106. if (float.IsNaN(num))
  107. {
  108. num = 0f;
  109. }
  110. constraintTarget.rotation = Quaternion.AngleAxis(num, axis) * constraintTarget.rotation;
  111. }
  112. }
  113. protected override void DoPlaneCorrect(Vector3 normal)
  114. {
  115. base.DoPlaneCorrect(normal);
  116. if (this.correctType == ALimbIKCtrl.BorderCorrectType.HalfBody)
  117. {
  118. Transform constraintTarget = this.shoulderThighCtrl.constraintTarget;
  119. constraintTarget.position += normal;
  120. if (!this.shoulderThighCtrl.pointIKData.isIKExec)
  121. {
  122. this.shoulderThighCtrl.positionWeight = base.positionWeight;
  123. }
  124. }
  125. }
  126. public override void OnPostFullBodySolverUpdate()
  127. {
  128. if (!base.isIKExec || this.handFootCtrl.isIKExecNotWeight0)
  129. {
  130. base.OnPostFullBodySolverUpdate();
  131. return;
  132. }
  133. if (base.rotateIKData.isIKExec)
  134. {
  135. this.shoulderThighCtrl.bone.rotation = Quaternion.Slerp(this.shoulderThighCtrl.bone.rotation, this.shoulderThighCtrl.constraintTarget.rotation, base.rotationWeight);
  136. base.bone.localRotation = this.BoneLocalRot;
  137. this.handFootCtrl.bone.localRotation = this.HandFootLocalRot;
  138. }
  139. Quaternion rotation = base.bone.rotation;
  140. if (base.pointIKData.isIKExec)
  141. {
  142. this.FABRIK.solver.Update();
  143. base.bone.rotation = rotation;
  144. }
  145. base.OnPostFullBodySolverUpdate();
  146. }
  147. protected override void OnPostSetPositionWeight(float val)
  148. {
  149. if (this.IsPullBody)
  150. {
  151. this.chain.bendConstraint.weight = val;
  152. }
  153. this.FABRIK.solver.IKPositionWeight = val;
  154. }
  155. private FABRIK FABRIK;
  156. private Quaternion HandFootLocalRot = Quaternion.identity;
  157. private Quaternion BoneLocalRot = Quaternion.identity;
  158. }