From ce8e0bff165d1f37bc9bb2adc72339999acc03a1 Mon Sep 17 00:00:00 2001 From: ousttrue Date: Fri, 14 May 2021 13:43:13 +0900 Subject: [PATCH] CalcYawPitch --- .../Constraint/VRM10AimConstraintEditor.cs | 49 ++++---- .../Constraint/VRM10AimConstraint.cs | 114 ++++-------------- 2 files changed, 49 insertions(+), 114 deletions(-) diff --git a/Assets/VRM10/Editor/Components/Constraint/VRM10AimConstraintEditor.cs b/Assets/VRM10/Editor/Components/Constraint/VRM10AimConstraintEditor.cs index 1c7f4fd2f..1521bda5d 100644 --- a/Assets/VRM10/Editor/Components/Constraint/VRM10AimConstraintEditor.cs +++ b/Assets/VRM10/Editor/Components/Constraint/VRM10AimConstraintEditor.cs @@ -52,35 +52,36 @@ namespace UniVRM10 Handles.DrawLine(m_target.Source.position, m_target.transform.position); var pos = m_target.transform.position; + TR.FromWorld(m_target.transform).Draw(0.2f); - EditorGUI.BeginChangeCheck(); - var pr = m_target.ParentRotation; + // EditorGUI.BeginChangeCheck(); + // var pr = m_target.ParentRotation; - if (m_target.Logic == null) - { - TR.FromWorld(m_target.transform).Draw(0.2f); + // if (m_target.Logic == null) + // { + // TR.FromWorld(m_target.transform).Draw(0.2f); - Handles.matrix = Matrix4x4.identity; - var rot = Handles.RotationHandle(pr * m_target.DestinationOffset, pos); - if (EditorGUI.EndChangeCheck()) - { - Undo.RecordObject(m_target, "Rotated RotateAt Point"); - m_target.DestinationOffset = Quaternion.Inverse(pr) * rot; - } + // Handles.matrix = Matrix4x4.identity; + // var rot = Handles.RotationHandle(pr * m_target.DestinationOffset, pos); + // if (EditorGUI.EndChangeCheck()) + // { + // Undo.RecordObject(m_target, "Rotated RotateAt Point"); + // m_target.DestinationOffset = Quaternion.Inverse(pr) * rot; + // } - DrawAimUp(rot, pos, Color.yellow); - } - else - { - var rot = m_target.ParentRotation * m_target.Logic.InitialLocalRotation * m_target.DestinationOffset; - DrawAimUp(rot, m_target.transform.position, Color.yellow); - DrawAimUp(m_target.transform.rotation * m_target.Delta, m_target.transform.position, Color.magenta); + // DrawAimUp(rot, pos, Color.yellow); + // } + // else + // { + // var rot = m_target.ParentRotation * m_target.Logic.InitialLocalRotation * m_target.DestinationOffset; + // DrawAimUp(rot, m_target.transform.position, Color.yellow); + // DrawAimUp(m_target.transform.rotation * m_target.Delta, m_target.transform.position, Color.magenta); - var sb = new StringBuilder(); - sb.AppendLine($"yaw: {m_target.Yaw:0.}"); - sb.AppendLine($"pitch: {m_target.Pitch:0.}"); - Handles.Label(m_target.transform.position, sb.ToString(), Style); - } + // var sb = new StringBuilder(); + // sb.AppendLine($"yaw: {m_target.Yaw:0.}"); + // sb.AppendLine($"pitch: {m_target.Pitch:0.}"); + // Handles.Label(m_target.transform.position, sb.ToString(), Style); + // } } } } diff --git a/Assets/VRM10/Runtime/Components/Constraint/VRM10AimConstraint.cs b/Assets/VRM10/Runtime/Components/Constraint/VRM10AimConstraint.cs index 3dc9f53d2..c9d4feee8 100644 --- a/Assets/VRM10/Runtime/Components/Constraint/VRM10AimConstraint.cs +++ b/Assets/VRM10/Runtime/Components/Constraint/VRM10AimConstraint.cs @@ -20,20 +20,20 @@ namespace UniVRM10 [Range(0, 10.0f)] public float Weight = 1.0f; - [SerializeField] - public Quaternion DestinationOffset = Quaternion.identity; - public Quaternion ParentRotation => transform.parent == null ? Quaternion.identity : transform.parent.rotation; - public class AimLogic - { - public readonly Quaternion InitialLocalRotation; - public AimLogic(Quaternion initialLocalRotation) - { - InitialLocalRotation = initialLocalRotation; - } - } - public AimLogic Logic { get; private set; } + // [SerializeField] + // public Quaternion DestinationOffset = Quaternion.identity; + + // public class AimLogic + // { + // public readonly Quaternion InitialLocalRotation; + // public AimLogic(Quaternion initialLocalRotation) + // { + // InitialLocalRotation = initialLocalRotation; + // } + // } + // public AimLogic Logic { get; private set; } void Start() @@ -44,75 +44,10 @@ namespace UniVRM10 return; } - if (Logic == null) - { - Logic = new AimLogic(transform.localRotation); - } - } - - static (float, int, float, int) CalcYawPitch(Matrix4x4 m, Vector3 target) - { - var zaxis = Vector3.Project(target, m.GetColumn(2)); - var yaxis = Vector3.Project(target, m.GetColumn(1)); - var xaxis = Vector3.Project(target, m.GetColumn(0)); - - var xDot = Vector3.Dot(xaxis, m.GetColumn(0)) > 0; - var yDot = Vector3.Dot(yaxis, m.GetColumn(1)) > 0; - var zDot = Vector3.Dot(zaxis, m.GetColumn(2)) > 0; - - // xz - var yaw = (float)System.Math.Atan2(xaxis.magnitude, zaxis.magnitude) * Mathf.Rad2Deg; - var yawQuadrant = -1; - if (xDot && zDot) - { - // 1st(0-90) - yawQuadrant = 0; - } - else if (xDot && !zDot) - { - // 2nd(90-180) - yawQuadrant = 1; - } - else if (!xDot && !zDot) - { - // 3rd - yawQuadrant = 2; - } - else if (!xDot && zDot) - { - // 4th - yawQuadrant = 3; - } - else - { - throw new NotImplementedException(); - } - - // xy - var pitch = (float)System.Math.Atan2(yaxis.magnitude, (xaxis + zaxis).magnitude) * Mathf.Rad2Deg; - var pitchQuadrant = -1; - if (yDot && zDot) - { - // 1st - pitchQuadrant = 0; - } - else if (yDot & !zDot) - { - // 2nd - pitchQuadrant = 1; - } - else if (!yDot & !zDot) - { - // 3rd - pitchQuadrant = 2; - } - else if (!yDot & zDot) - { - // 4th - pitchQuadrant = 3; - } - - return (yaw, yawQuadrant, pitch, pitchQuadrant); + // if (Logic == null) + // { + // Logic = new AimLogic(transform.localRotation); + // } } /// @@ -121,16 +56,15 @@ namespace UniVRM10 /// public override void Process() { - if (Logic == null) - { - return; - } + // if (Logic == null) + // { + // return; + // } - var m = Matrix4x4.TRS(transform.position, ParentRotation * Logic.InitialLocalRotation, Vector3.one); - m.CalcYawPitch(Source.position, out Yaw, out Pitch); - // Delta = Quaternion.Euler(0, Yaw, 0) * Quaternion.Euler(Pitch, 0, 0); - Delta = m.YawPitchRotation(Yaw, Pitch); - transform.rotation = ParentRotation * Logic.InitialLocalRotation * Delta; + var m = Matrix4x4.Rotate(ParentRotation); + m.CalcYawPitch(Source.position - transform.position, out Yaw, out Pitch); + Delta = Quaternion.Euler(0, Yaw, 0) * Quaternion.Euler(-Pitch, 0, 0); + transform.rotation = ParentRotation * Delta; } public float Yaw;