示例#1
0
 public static double X(this MTranslationConstraint tConstraint)
 {
     return(tConstraint.Limits.X());
 }
示例#2
0
        public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
        {
            //Assign the instruction
            this.instruction = instruction;

            MBoolResponse response = new MBoolResponse(true);



            if (instruction.Constraints.Count > 0 && instruction.Constraints[0].GeometryConstraint != null)
            {
                MSceneObject parent = this.SceneAccess.GetSceneObjectByID(instruction.Constraints[0].GeometryConstraint.ParentObjectID);

                if (instruction.Constraints[0].GeometryConstraint.ParentToConstraint != null)
                {
                    MTransform ptc = instruction.Constraints[0].GeometryConstraint.ParentToConstraint;
                    String     gtp = ptc.Parent;
                    if (gtp != null && this.SceneAccess.GetSceneObjectByID(gtp) != null)
                    {
                        // transform parent takes precedent.
                        this.targetTransform = ptc.LocalToGlobal(this.SceneAccess);
                    }
                    else if (parent != null)
                    {
                        // parent to constraint has not valid parent, thus the geometry constraint parent is
                        this.targetTransform = ptc.Multiply(parent.Transform.LocalToGlobal(this.SceneAccess));
                    }
                    else
                    {
                        this.targetTransform = ptc;
                    }
                }
                else
                {
                    MVector3 pos = new MVector3(0, 0, 0);
                    if (instruction.Constraints[0].GeometryConstraint.TranslationConstraint != null)
                    {
                        MTranslationConstraint trlCstr = instruction.Constraints[0].GeometryConstraint.TranslationConstraint;
                        if (parent != null)
                        {
                            pos = parent.Transform.Position.Add(trlCstr.GetVector3());
                        }
                        else
                        {
                            pos = trlCstr.GetVector3();
                        }
                    }
                    MQuaternion rot = new MQuaternion(0, 0, 0, 1);
                    if (instruction.Constraints[0].GeometryConstraint.RotationConstraint != null)
                    {
                        MRotationConstraint rtCstr = instruction.Constraints[0].GeometryConstraint.RotationConstraint;
                        if (parent != null)
                        {
                            rot = rtCstr.GetQuaternion().Multiply(parent.Transform.Rotation);
                        }
                        else
                        {
                            rot = rtCstr.GetQuaternion();
                        }
                    }
                    this.targetTransform = new MTransform("", pos, rot);
                }
            }
            else
            {
                response = new MBoolResponse(false)
                {
                    LogData = new List <string>()
                    {
                        "Required target constraint (MGeometryConstraint) not defined"
                    }
                };
            }



            //Extract the velocity if defined
            if (instruction.Properties.ContainsKey("Velocity"))
            {
                Console.WriteLine("vel: " + instruction.Properties["Velocity"]);
                float.TryParse(instruction.Properties["Velocity"], System.Globalization.NumberStyles.Float, System.Globalization.CultureInfo.InvariantCulture, out velocity);
            }
            else
            {
                velocity = -1.0f;
            }

            /*
             * //Get the target id
             * if (instruction.Properties.ContainsKey("TargetID"))
             *  this.targetTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]);
             * //Error id not available
             * else
             * {
             *  return new MBoolResponse(false)
             *  {
             *  };
             * }*/

            //Return true/success
            return(response);
        }
示例#3
0
 public static MVector3 GetVector3(this MTranslationConstraint tConstraint)
 {
     return(new MVector3(tConstraint.X(), tConstraint.Y(), tConstraint.Z()));
 }
示例#4
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);
        }