/// <summary>
        /// Visualizes the constraints
        /// </summary>
        /// <param name="constraints"></param>
        private void ShowConstraints(List <MConstraint> constraints)
        {
            for (int i = constraintObjects.Count - 1; i >= 0; i--)
            {
                constraintObjects[i].SetActive(false);
            }

            int index = 0;

            foreach (MConstraint constraint in constraints)
            {
                if (constraint.JointConstraint != null)
                {
                    MJointConstraint endeffectorConstraint = constraint.JointConstraint;

                    if (endeffectorConstraint.GeometryConstraint != null)
                    {
                        MTranslationConstraint posConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint;

                        GameObject p = null;
                        if (index < constraintObjects.Count)
                        {
                            p = constraintObjects[index];
                            p.SetActive(true);
                        }
                        else
                        {
                            p = GameObject.CreatePrimitive(PrimitiveType.Sphere);
                            p.GetComponent <Renderer>().material       = new Material(Shader.Find("Standard"));
                            p.GetComponent <Renderer>().material.color = Color.red;
                            p.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
                            constraintObjects.Add(p);
                        }

                        index++;

                        p.transform.position = new Vector3((float)posConstraint.X(), (float)posConstraint.Y(), (float)posConstraint.Z());
                    }
                }
            }
        }
Exemplo n.º 2
0
 public static MVector3 GetVector3(this MTranslationConstraint tConstraint)
 {
     return(new MVector3(tConstraint.X(), tConstraint.Y(), tConstraint.Z()));
 }
Exemplo n.º 3
0
        /// <summary>
        /// Returns all ik constraints which are violated (avove specified threshold)
        /// </summary>
        /// <param name="constraints"></param>
        /// <param name="currentPosture"></param>
        /// <returns></returns>
        private List <MConstraint> GetViolatedIKConstraints(List <MConstraint> constraints, MAvatarPostureValues currentPosture)
        {
            List <MConstraint> violated = new List <MConstraint>();

            if (constraints != null)
            {
                // Apply result posture values to the skeleton
                skeletonAccess.SetChannelData(currentPosture);

                string avatarID = currentPosture.AvatarID;

                //Check each joint constraint
                foreach (MConstraint mconstraint in constraints)
                {
                    if (mconstraint.JointConstraint != null)
                    {
                        MJointConstraint endeffectorConstraint = mconstraint.JointConstraint;

                        //Skip if no gemometry constraint is defined
                        if (endeffectorConstraint.GeometryConstraint == null)
                        {
                            continue;
                        }


                        double distance        = 0f;
                        double angularDistance = 0f;


                        //Default (parent to constraint is set)
                        if (endeffectorConstraint.GeometryConstraint.ParentToConstraint != null)
                        {
                            MVector3    position = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Position;
                            MQuaternion rotation = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Rotation;

                            switch (endeffectorConstraint.JointType)
                            {
                            case MJointType.LeftWrist:
                                distance        = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist));
                                angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist));

                                break;

                            case MJointType.RightWrist:
                                distance        = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist));
                                angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist));

                                break;

                            case MJointType.LeftBall:
                                distance        = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle));
                                angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle));

                                break;

                            case MJointType.RightBall:
                                distance        = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle));
                                angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle));

                                break;

                            case MJointType.PelvisCentre:
                                distance        = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre));
                                angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre));

                                break;
                            }
                        }

                        //Legacy fallback mechanism -> Remove in future
                        else
                        {
                            MTranslationConstraint positionConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint;


                            if (endeffectorConstraint.GeometryConstraint.TranslationConstraint != null)
                            {
                                switch (endeffectorConstraint.JointType)
                                {
                                case MJointType.LeftWrist:
                                    distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist));
                                    break;

                                case MJointType.RightWrist:
                                    distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist));
                                    break;

                                case MJointType.LeftBall:
                                    distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle));
                                    break;

                                case MJointType.RightBall:
                                    distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle));
                                    break;

                                case MJointType.PelvisCentre:
                                    distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre));
                                    break;
                                }
                            }

                            //Handle the rotation constraint
                            if (endeffectorConstraint.GeometryConstraint.RotationConstraint != null)
                            {
                                MRotationConstraint rotationConstraint = endeffectorConstraint.GeometryConstraint.RotationConstraint;

                                //Compute a quaternion based on the euler angles
                                MQuaternion quaternion = MQuaternionExtensions.FromEuler(new MVector3(rotationConstraint.X(), rotationConstraint.Y(), rotationConstraint.Z()));

                                if (endeffectorConstraint.GeometryConstraint.ParentObjectID == null || endeffectorConstraint.GeometryConstraint.ParentObjectID == "")
                                {
                                    switch (endeffectorConstraint.JointType)
                                    {
                                    case MJointType.LeftWrist:
                                        angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist));
                                        break;

                                    case MJointType.RightWrist:
                                        angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist));
                                        break;

                                    case MJointType.LeftBall:
                                        angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle));
                                        break;

                                    case MJointType.RightBall:
                                        angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle));
                                        break;

                                    case MJointType.PelvisCentre:
                                        angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre));
                                        break;
                                    }
                                }
                            }
                        }

                        //Check if solving is required
                        if (distance > this.PositionThreshold || angularDistance > this.RotationThreshold)
                        {
                            violated.Add(mconstraint);
                        }
                    }
                }
            }

            return(violated);
        }