예제 #1
0
 private void _UpdateTargetCollider()
 {
     if (m_target)
     {
         m_targetCollider = m_target.GetComponent <Collider>();
         if (m_targetCollider == null)
         {
             m_useRaycast = false;
         }
     }
     else
     {
         Dbg.CLogWarn(this, "Floor._UpdateTargetCollider: not set m_target yet: {0}", name);
     }
 }
예제 #2
0
        public override void Apply(ISolver solver, int jointIdx)
        {
            if (m_nextJoint == null)
            {
                Dbg.CLogWarn(this, "AngleConstraintMB.Apply: nextJoint not set");
                return;
            }

            if (jointIdx == solver.Count)
            {
                return; //if no child joint, cannot apply angle constraint
            }
            var       joints = solver.GetJoints();
            Transform j      = joints[jointIdx];

            if (m_rotAxis == Vector3.zero)
            {
                Dbg.LogErr("AngleConstraintMB.Apply: the axis is zero vector");
                return;
            }
            if (m_minLimit > m_maxLimit)
            {
                Misc.Swap(ref m_minLimit, ref m_maxLimit);
            }

            Transform nextJ        = joints[jointIdx + 1];
            Transform parentJ      = j.parent; //THIS could be NULL, use Misc.TransformDirectoin/InverseTransformDirection
            Vector3   jpos         = j.position;
            Vector3   nextJpos     = nextJ.position;
            Vector3   rotAxisWorld = Misc.TransformDirection(parentJ, m_rotAxis); //axis convert from parentSpace to worldSpace

            // project to the rotation plane
            Vector3 diff0         = nextJpos - jpos;                             //world space
            Vector3 projDiff      = Vector3.ProjectOnPlane(diff0, rotAxisWorld); //world space
            Vector3 worldPrimAxis = Misc.TransformDirection(parentJ, m_primAxis);

            float angle = Misc.ToAngleAxis(worldPrimAxis, projDiff, rotAxisWorld);

            if (angle < m_minLimit || m_maxLimit < angle)
            { //need clamp
                angle = Mathf.Clamp(angle, m_minLimit, m_maxLimit);
            }

            j.localRotation = Quaternion.AngleAxis(angle, m_rotAxis) * m_startLocalRot; //local
        }
예제 #3
0
        // private method

        private void _OnNextJointChanged()
        {
            AngleConstraintMB mb = this;
            var nextJoint        = mb.m_nextJoint;

            if (nextJoint != null)
            {
                if (nextJoint.parent == mb.transform)
                {
                    AutoSetParameters();
                }
                else
                {
                    Dbg.CLogWarn(mb, nextJoint.name + " is not children of " + mb.name);
                    mb.m_nextJoint = null;
                }
            }
        }
예제 #4
0
    public static void CLogWarn <T1, T2, T3, T4>(Object ctx, string fmt, T1 par1, T2 par2, T3 par3, T4 par4)
    {
        string msg = string.Format(fmt, par1, par2, par3, par4);

        Dbg.CLogWarn(ctx, msg);
    }
예제 #5
0
    public static void CLogWarn <T1, T2>(Object ctx, string fmt, T1 par1, T2 par2)
    {
        string msg = string.Format(fmt, par1, par2);

        Dbg.CLogWarn(ctx, msg);
    }
예제 #6
0
        /// <summary>
        /// 1. rotate bone back from current rotation to refAxis; angle is "X"
        /// 2. clamp X;
        /// 3. rotate back with clamped X;
        /// 4. calculate and clamp twist;
        /// </summary>
        public override void Apply(ISolver solver, int jointIdx)
        {
            if (m_nextJoint == null)
            {
                Dbg.CLogWarn(this, "ConeConstraintMB.Apply: nextJoint not set: {0}", name);
                return;
            }
            Dbg.CAssert(this, m_angleLimit >= 0, "ConeConstraintMB.Apply: m_angleLimit should >= 0, but: {0}", m_angleLimit);
            Dbg.CAssert(this, -180f <= m_minTwistLimit && m_minTwistLimit <= 180f, "ConeConstraintMB.Apply: minTwistLimit: {0}", m_minTwistLimit);
            Dbg.CAssert(this, -180f <= m_maxTwistLimit && m_maxTwistLimit <= 180f, "ConeConstraintMB.Apply: maxTwistLimit: {0}", m_maxTwistLimit);
            Dbg.CAssert(this, 0 <= m_angleLimit && m_angleLimit <= 180f, "ConeConstraintMB.Apply: angleLimit: {0}", m_angleLimit);

            var       joints = solver.GetJoints();
            Transform j      = joints[jointIdx];
            Transform cj     = m_nextJoint;
            Transform pj     = j.parent;

            //1
            Vector3    boneDirWorld = cj.position - j.position;
            Vector3    refDirWorld = Misc.TransformDirection(pj, m_refAxis);
            Quaternion q = Quaternion.FromToRotation(boneDirWorld, refDirWorld);
            float      angle; Vector3 rotAxis;

            q.ToAngleAxis(out angle, out rotAxis);
            angle = Misc.NormalizeAnglePI(angle);

            j.rotation = q * j.rotation;
            //Dbg.Log("coneconstraint: angle: {0}, rotAxis: {1}", angle, rotAxis);

            //2
            angle = Mathf.Clamp(angle, -m_angleLimit, m_angleLimit);

            //3
            j.Rotate(rotAxis, -angle, Space.World);

            //4
            if (m_limitTwist)
            { //use swing-twist decomposition of quaternion to limit twist
                Quaternion deltaRot = j.localRotation * Quaternion.Inverse(m_initRot);

                Vector3    curTwistAxisDirWorld  = (m_nextJoint.position - tr.position).normalized;
                Vector3    curTwistAxisDirParent = Misc.InverseTransformDirection(pj, curTwistAxisDirWorld);
                Quaternion swingRot = Quaternion.FromToRotation(m_twistAxis, curTwistAxisDirParent);
                Quaternion twistRot = Quaternion.Inverse(swingRot) * deltaRot;

                Vector3 tmpAxis; float twist;
                twistRot.ToAngleAxis(out twist, out tmpAxis);
                twist = Misc.NormalizeAnglePI(twist);
                if (float.IsInfinity(tmpAxis.x)) //SPECIAL case, some extreme data will make tmpAxis to be <inf,inf,inf>
                {
                    tmpAxis = Vector3.right;
                    twist   = 0;
                }
                if (Misc.IsObtuseAngle(tmpAxis, m_twistAxis))
                {
                    twist   = -twist;
                    tmpAxis = -tmpAxis;
                }

                twist    = Mathf.Clamp(twist, m_minTwistLimit, m_maxTwistLimit);
                twistRot = Quaternion.AngleAxis(twist, tmpAxis);
                deltaRot = swingRot * twistRot;

                var applied = deltaRot * m_initRot;
                j.localRotation = applied;
            }
        }