public static MVector3 GetVector3(this MRotationConstraint rConstraint)
 {
     return(new MVector3(rConstraint.X(), rConstraint.Y(), rConstraint.Z()));
 }
Exemple #2
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);
        }