123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407 |
- using System;
- using System.Collections.Generic;
- using kt.ik;
- using kt.Utility;
- using UnityEngine;
- [Serializable]
- public class PenisIKCtrl : AIKCtrl
- {
- public PenisIKCtrl(FullBodyIKMgr ik_mgr) : base(ik_mgr, FullBodyIKMgr.IKEffectorType.Penis)
- {
- base.isUpperBody = false;
- foreach (Transform transform in base.chainBones)
- {
- this.BoneDataDic[transform] = new PenisIKCtrl.BoneData(transform);
- }
- }
- public AutoKupaCtrl kupaCtrl { get; private set; }
- public override void Reset()
- {
- base.Reset();
- this.LastIsInserted = this.IsInserted;
- this.IsInserted = false;
- this.IsNotEscape = false;
- foreach (PenisIKCtrl.BoneData boneData in this.BoneDataDic.Values)
- {
- boneData.isInsertedOnce = false;
- boneData.lastIsScaleVariable = boneData.isScaleVariable;
- if (boneData.isScaleVariable)
- {
- boneData.isScaleVariable = false;
- boneData.bone.localScale = Vector3.one;
- }
- }
- }
- public override void SetIKSetting(IKAttachParam param)
- {
- PenisIKParam penisIKParam = param as PenisIKParam;
- this.insertBeforeAxis = penisIKParam.insertBefore;
- this.insertAfterAxis = penisIKParam.insertAfter;
- this.sensorLength = penisIKParam.sensorLength;
- this.IsNotEscape = penisIKParam.isNotEscape;
- this.IsForciblyBend = penisIKParam.isForciblyBend;
- this.AlwaysLookTarget = penisIKParam.alwaysLookTarget;
- if (this.LastIsInserted)
- {
- this.IsInserted |= (base.GetIKSettingData(param.attachType).curTargetData.target == param.attachTarget);
- }
- base.SetIKSetting(param);
- this.kupaCtrl = penisIKParam.kupaCtrl;
- if (this.kupaCtrl != null)
- {
- this.kupaCtrl.SetKupaInsert(base.chainBones, this.insertBeforeAxis);
- this.kupaCtrl.isUpdateTBody = false;
- }
- foreach (KeyValuePair<string, float> keyValuePair in penisIKParam.boneNameScaleValPair)
- {
- Transform bone = this.myIKMgr.body.GetBone(keyValuePair.Key);
- this.BoneDataDic[bone].isScaleVariable = true;
- this.BoneDataDic[bone].scaleValue = keyValuePair.Value;
- }
- this.MotionCtrlBoneName = penisIKParam.motionCtrlBoneName;
- this.MotionCtrlWeight = penisIKParam.motionCtrlWeight;
- }
- public override void Detach()
- {
- if (this.kupaCtrl != null && this.kupaCtrl.morphInsert.chainEndBone == base.bone)
- {
- this.kupaCtrl.SetKupaInsert(AutoKupaCtrl.KupaType.Close, base.chainBones, this.insertBeforeAxis);
- this.kupaCtrl.isUpdateTBody = true;
- }
- this.kupaCtrl = null;
- this.IsInserted = false;
- this.IsNotEscape = false;
- foreach (PenisIKCtrl.BoneData boneData in this.BoneDataDic.Values)
- {
- if (boneData.bone)
- {
- boneData.bone.localPosition = boneData.initLocalPos;
- }
- boneData.isScaleVariable = boneData.lastIsScaleVariable;
- }
- base.Detach();
- }
- protected override void SetTargetTransform(AIKCtrl.IKSettingData data, Vector3 pos, Quaternion rot)
- {
- if (data.blendType == AIKCtrl.IKBlendType.IK_To_Detach && data.curTargetData.lastTarget)
- {
- pos = data.curTargetData.lastTarget.position;
- rot = data.curTargetData.lastTarget.rotation;
- }
- base.SetTargetTransform(data, pos, rot);
- if (!data.isPointAttach)
- {
- return;
- }
- base.constraintTarget.rotation = rot;
- Vector3 vector = rot * this.insertBeforeAxis;
- Vector3 vector2 = rot * this.insertAfterAxis;
- for (int i = 1; i < base.chainBones.Length; i++)
- {
- Transform transform = base.chainBones[i];
- if (this.BoneDataDic[transform].isScaleVariable)
- {
- transform.localScale = Vector3.one;
- }
- transform.localPosition = this.BoneDataDic[transform].initLocalPos;
- Transform transform2 = base.chainBones[i - 1];
- Color magenta = Color.magenta;
- magenta.a = base.positionWeight;
- Debug.DrawLine(transform2.position, transform.position, magenta);
- }
- if ((this.IsNotEscape && this.IsForciblyBend) || this.AlwaysLookTarget)
- {
- Vector3 fromDirection = base.chainBones[1].position - base.chainBones[0].position;
- Vector3 toDirection = base.constraintTarget.position - base.chainBones[0].position;
- Quaternion quaternion = Quaternion.FromToRotation(fromDirection, toDirection) * base.chainBones[0].rotation;
- if (this.AlwaysLookTarget)
- {
- quaternion = Quaternion.Lerp(base.chainBones[0].rotation, quaternion, base.positionWeight);
- }
- Debug.DrawLine(base.chainBones[0].position, base.bone.position, UnityUtility.SetAlpha(Color.cyan, base.positionWeight));
- base.chainBones[0].rotation = quaternion;
- }
- float num = Vector3.Dot(base.bone.position - base.constraintTarget.position, vector);
- if (!this.IsNotEscape || !this.IsInserted)
- {
- this.IsInserted = (num <= 0f);
- if (this.IsInserted)
- {
- for (int j = 1; j < base.chainBones.Length; j++)
- {
- Transform transform3 = base.chainBones[j];
- float num2 = Vector3.Dot(transform3.position - base.constraintTarget.position, vector);
- if (num2 <= 0f)
- {
- this.IsInserted &= this.IsInsertRangeIn(transform3.position);
- break;
- }
- }
- }
- }
- Debug.DrawRay(pos, vector * 0.025f, UnityUtility.SetAlpha(Color.blue, base.positionWeight));
- Debug.DrawRay(pos, vector2 * 0.025f, UnityUtility.SetAlpha(Color.red, base.positionWeight));
- if (!this.IsInserted)
- {
- base.positionWeight = 0f;
- }
- else if (this.sensorLength > 0f && num > 0f)
- {
- base.positionWeight *= 1f - num / this.sensorLength;
- }
- if (!this.IsNotEscape && this.IsForciblyBend)
- {
- Vector3 fromDirection2 = base.chainBones[1].position - base.chainBones[0].position;
- Vector3 toDirection2 = base.constraintTarget.position - base.chainBones[0].position;
- Quaternion quaternion2 = Quaternion.FromToRotation(fromDirection2, toDirection2) * base.chainBones[0].rotation;
- quaternion2 = Quaternion.Lerp(base.chainBones[0].rotation, quaternion2, base.positionWeight);
- Debug.DrawLine(base.chainBones[0].position, base.bone.position, UnityUtility.SetAlpha(Color.cyan, base.positionWeight));
- base.chainBones[0].rotation = quaternion2;
- }
- num -= this.sensorLength;
- bool flag = true;
- float num3 = -1f;
- bool flag2 = false;
- for (int k = 1; k < base.chainBones.Length; k++)
- {
- Transform transform4 = base.chainBones[k];
- PenisIKCtrl.BoneData boneData = this.BoneDataDic[transform4];
- Transform transform5 = base.chainBones[k - 1];
- if (k == 1)
- {
- this.BoneDataDic[transform5].orijinPos = transform5.position;
- this.BoneDataDic[transform5].solvePos = transform5.position;
- }
- Vector3 solvePos = this.BoneDataDic[transform5].solvePos;
- Vector3 vector3;
- if (this.MotionCtrlWeight < 1f)
- {
- Matrix4x4 localToWorldMatrix = transform5.localToWorldMatrix;
- localToWorldMatrix.SetColumn(3, transform5.position);
- vector3 = localToWorldMatrix.MultiplyPoint3x4(transform4.localPosition);
- }
- else
- {
- vector3 = transform4.position;
- }
- float num4 = Vector3.Dot(vector3 - base.constraintTarget.position, vector);
- Vector3 vector4 = Vector3.zero;
- boneData.insertValueSelf = 0f;
- boneData.insertValueParent = 0f;
- if (!flag2)
- {
- flag2 = (this.MotionCtrlBoneName == transform4.name);
- }
- if (num4 <= 0f)
- {
- float num5 = (!flag) ? (Mathf.Abs(num4) / (Mathf.Abs(num4) + num3)) : 1f;
- Vector3 a = base.constraintTarget.position + vector * num4;
- Vector3 b = base.constraintTarget.position + vector2 * Vector3.Dot(vector3 - base.constraintTarget.position, vector2);
- vector4 = Vector3.Lerp(a, b, num5);
- boneData.insertValueSelf = 1f;
- boneData.insertValueParent = num5;
- flag = true;
- }
- else
- {
- Vector3 vector5 = base.constraintTarget.position + vector * num4;
- float num5 = Mathf.Abs(num) / (num4 + Mathf.Abs(num));
- if (this.IsNotEscape && this.IsForciblyBend)
- {
- vector4 = vector5;
- }
- else
- {
- vector4 = Vector3.Lerp(vector3, vector5, num5);
- }
- boneData.insertValueSelf = num5;
- flag = false;
- }
- if (!boneData.isInsertedOnce && !data.isBlendNow)
- {
- boneData.isInsertedOnce = (num4 <= 0f);
- }
- this.BoneDataDic[transform4].orijinPos = vector3;
- float d = Vector3.Distance(transform5.position, transform4.position);
- if (flag2)
- {
- vector4 = Vector3.Lerp(vector4, vector3, this.MotionCtrlWeight);
- }
- if (this.MotionCtrlWeight < 1f)
- {
- Vector3 a2 = vector4 - solvePos;
- float magnitude = a2.magnitude;
- if (magnitude > 0f)
- {
- vector4 = solvePos + a2 / magnitude * d;
- }
- }
- this.BoneDataDic[transform4].solvePos = vector4;
- num3 = num4;
- }
- }
- private bool IsInsertRangeIn(Vector3 position)
- {
- Vector3 lhs = position - base.constraintTarget.position;
- Quaternion rotation = Quaternion.FromToRotation(Vector3.forward, base.constraintTarget.rotation * this.insertBeforeAxis);
- Vector3 rhs = rotation * Vector3.up;
- Vector3 rhs2 = rotation * Vector3.right;
- float num = Mathf.Abs(Vector3.Dot(lhs, rhs));
- float num2 = Mathf.Abs(Vector3.Dot(lhs, rhs2));
- return num <= 0.05f && num2 <= 0.05f;
- }
- public override void OnPostFullBodySolverUpdate()
- {
- if (!base.pointIKData.isIKExec)
- {
- return;
- }
- for (int i = 1; i < base.chainBones.Length; i++)
- {
- Transform transform = base.chainBones[i - 1];
- Transform transform2 = base.chainBones[i];
- PenisIKCtrl.BoneData boneData = this.BoneDataDic[transform2];
- Vector3 orijinPos = boneData.orijinPos;
- Vector3 solvePos = boneData.solvePos;
- Vector3 a = Vector3.Lerp(orijinPos, solvePos, base.positionWeight);
- Vector3 fromDirection = transform2.position - transform.position;
- Vector3 toDirection = a - transform.position;
- transform.rotation = Quaternion.FromToRotation(fromDirection, toDirection) * transform.rotation;
- if (boneData.isScaleVariable)
- {
- Vector3 vector = new Vector3(1f, boneData.scaleValue, boneData.scaleValue);
- if (base.pointIKData.blendType != AIKCtrl.IKBlendType.IK_To_Detach)
- {
- if (!boneData.isInsertedOnce)
- {
- transform2.localScale = Vector3.Lerp(Vector3.one, vector, boneData.insertValueSelf * base.positionWeight);
- }
- else
- {
- transform2.localScale = vector;
- }
- }
- else
- {
- transform2.localScale = Vector3.Lerp(Vector3.one, vector, boneData.insertValueSelf * base.positionWeight);
- }
- }
- }
- if (this.kupaCtrl != null)
- {
- this.kupaCtrl.KupaUpdate();
- }
- if (this.IsInserted && this.IsNotEscape)
- {
- bool flag = false;
- AIKCtrl.PosRotPair posRotPair = default(AIKCtrl.PosRotPair);
- for (int j = 1; j < base.chainBones.Length; j++)
- {
- Transform transform3 = base.chainBones[j];
- Vector3 rhs = base.constraintTarget.rotation * this.insertBeforeAxis;
- float num = Vector3.Dot(transform3.position - base.constraintTarget.position, rhs);
- if (!flag && num <= 0f)
- {
- flag = true;
- posRotPair.Copy(transform3);
- }
- if (flag)
- {
- transform3.position = posRotPair.pos;
- transform3.rotation = posRotPair.rot;
- }
- else if (j == base.chainBones.Length - 2)
- {
- transform3.position = base.constraintTarget.position;
- break;
- }
- }
- }
- }
- public void PenisRotateReset()
- {
- for (int i = 1; i < base.chainBones.Length; i++)
- {
- Transform transform = base.chainBones[i];
- transform.localPosition = this.BoneDataDic[transform].initLocalPos;
- transform.localRotation = this.BoneDataDic[transform].initLocalRot;
- }
- }
- private const float INSERT_RANGE_MAX = 0.05f;
- private Dictionary<Transform, PenisIKCtrl.BoneData> BoneDataDic = new Dictionary<Transform, PenisIKCtrl.BoneData>();
- public Vector3 insertBeforeAxis = Vector3.back;
- public Vector3 insertAfterAxis = Vector3.forward;
- [Space]
- public float sensorLength;
- private bool IsInserted;
- private bool LastIsInserted;
- [SerializeField]
- [Header("ちんこを抜けないようにする")]
- private bool IsNotEscape;
- [SerializeField]
- [Header("ちんこを無理やり曲げる")]
- private bool IsForciblyBend;
- [SerializeField]
- private bool AlwaysLookTarget;
- [Space]
- [SerializeField]
- [Header("モーション側で制御するボーン名")]
- private string MotionCtrlBoneName = string.Empty;
- [SerializeField]
- [Header("制御のウェイト")]
- [Range(0f, 1f)]
- private float MotionCtrlWeight;
- private class BoneData
- {
- public BoneData(Transform bone)
- {
- this.bone = bone;
- this.initLocalPos = bone.localPosition;
- this.initLocalRot = bone.localRotation;
- }
- public readonly Transform bone;
- public readonly Vector3 initLocalPos;
- public readonly Quaternion initLocalRot;
- public bool isInsertedOnce;
- public float insertValueSelf;
- public float insertValueParent;
- public Vector3 orijinPos;
- public Vector3 solvePos;
- public bool isScaleVariable;
- public bool lastIsScaleVariable;
- public float scaleValue = 1f;
- }
- }
|