Exemple #1
0
        public void SetToPosition(TransformMatrix tm)
        {
            if (_bone.hasTransform())
            {
                _bone.removeTransform();
            }

            if (tm.isIdentity())
            {
                return; //nothing to do in this case
            }
            else
            {
                _bone.addTransform(tm.ToTransform());
            }
        }
Exemple #2
0
        public TransformMatrix[] CalculateInterpolatedMotion(int startPositionIndex, int endPositionIndex, Bone startFixedBone, Bone endFixedBone, int numSteps)
        {
            TransformMatrix[] finalTransforms = new TransformMatrix[numSteps];
            TransformMatrix   startTransform  = this.CalculateRelativeMotionFromNeutral(startPositionIndex, startFixedBone);
            TransformMatrix   endTransform    = this.CalculateRelativeMotionFromNeutral(endPositionIndex, endFixedBone);

            TransformMatrix  relMotion   = endTransform * startTransform.Inverse();
            HelicalTransform relMotionHT = relMotion.ToHelical();

            HelicalTransform[] htTransforms = relMotionHT.LinearlyInterpolateMotion(numSteps);

            for (int i = 0; i < numSteps; i++)
            {
                finalTransforms[i] = htTransforms[i].ToTransformMatrix() * startTransform;
            }

            return(finalTransforms);
        }
Exemple #3
0
        public TransformMatrix CalculateRelativeMotionFromNeutral(int positionIndex, Bone fixedBone)
        {
            //check for a neutral position
            if (positionIndex == 0)
            {
                return(new TransformMatrix()); //no motion in neutral
            }
            //check for the fixed bone being us :)
            if (this == fixedBone)
            {
                //TODO: What do I do in this case...?
                return(new TransformMatrix());
            }

            TransformMatrix tmFixedBoneInverse = fixedBone._transformMatrices[positionIndex].Inverse();

            return(tmFixedBoneInverse * _transformMatrices[positionIndex]);
        }
Exemple #4
0
        public void createFromQuaternions(double q0, double q1, double q2, double q3, double[] center)
        {
            if (center.Length != 3)
            {
                throw new ArgumentException("Center point must have 3 values");
            }

            TransformMatrix rotMat          = new TransformMatrix();
            TransformMatrix transMat        = new TransformMatrix();
            TransformMatrix transMatInverse = new TransformMatrix();


            rotMat.quaternionToRotationMatrix(q0, q1, q2, q3);
            transMat.setTranslation(center);
            transMatInverse.setTranslation(-center[0], -center[1], -center[2]);
            TransformMatrix final = transMat * rotMat * transMatInverse;

            SetMatrix(final);
        }
Exemple #5
0
        public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS, TransformMatrix BoneRelMation)
        {
            TransformMatrix relativeCSMotion = UCS.Inverse() * BoneRelMation * RCS;

            double[]            euler  = relativeCSMotion.ToEuler();
            PronationSupination result = new PronationSupination();

            result.Euler_x = euler[0];
            result.Euler_y = euler[1];
            result.Euler_z = euler[2];
            double pronationAngle = euler[0] - NEUTRAL_PS_POSITION; //correct for offset

            //correct for negative angles, want final wrist position to be [-180 180]
            while (pronationAngle < -180)
            {
                pronationAngle += 360;
            }
            result.PronationAngle = pronationAngle;
            return(result);
        }
Exemple #6
0
        public static TransformMatrix[][] parseXrommKinematicFileToTransformMatrix(string filename)
        {
            double[][] dat = parseDatFile(filename);
            if (dat.Length == 0 || dat[0].Length == 0)
            {
                return(null);
            }
            if (dat[0].Length % 16 != 0)
            {
                throw new ArgumentException("Invalid Xromm data file format. Column number must be multiple of 16!");
            }
            int numCol   = dat[0].Length;
            int numBones = dat[0].Length / 16;

            TransformMatrix[][] transforms = new TransformMatrix[numBones][];
            for (int i = 0; i < numBones; i++)
            {
                transforms[i] = new TransformMatrix[dat.Length];
            }

            for (int i = 0; i < dat.Length; i++)
            {
                if (dat[i].Length != numCol)
                {
                    throw new ArgumentException("Invalid XROMM data file format. Every row must have the same number of columns");
                }
                for (int j = 0; j < numBones; j++)
                {
                    if (Double.IsNaN(dat[i][j * 16]))
                    {
                        //what do we do for NaN's.... for now we leave blank...yes?
                        //so do nothing
                    }
                    else
                    {
                        transforms[j][i] = new TransformMatrix(dat[i], 16 * j);
                    }
                }
            }
            return(transforms);
        }
Exemple #7
0
        public static Switch CreateAnimationSwitch(Bone bone, Bone fixedBone, int[] animationOrder, int numFrames)
        {
            Switch sw = new Switch();

            sw.reference();

            //add starting position, each animation does the animation and the ending frame, not the starting one :)
            TransformMatrix startPosition = bone.CalculateRelativeMotionFromNeutral(animationOrder[0], fixedBone);

            sw.addChild(startPosition.ToTransform());
            for (int i = 0; i < animationOrder.Length - 1; i++) //not the last animation, there need start and end
            {
                TransformMatrix[] tmTransforms = bone.CalculateInterpolatedMotion(animationOrder[i], animationOrder[i + 1], fixedBone, numFrames);
                foreach (TransformMatrix tform in tmTransforms)
                {
                    sw.addChild(tform.ToTransform());
                }
            }
            sw.unrefNoDelete();
            return(sw);
        }
Exemple #8
0
 /// <summary>
 /// Compares two TransformMatrices to see if they are equal
 /// </summary>
 /// <param name="B">Matrix to test against</param>
 /// <returns></returns>
 public bool isEqual(TransformMatrix B)
 {
     return(isEqual(B, 0.0001f));
 }
Exemple #9
0
 /// <summary>Linear algebraic matrix multiplication, A * B</summary>
 /// <param name="B">   another matrix
 /// </param>
 /// <returns>     Matrix product, A * B
 /// </returns>
 /// <exception cref="System.ArgumentException">  Matrix inner dimensions must agree.
 /// </exception>
 public virtual TransformMatrix Multiply(TransformMatrix B)
 {
     return(new TransformMatrix(base.Multiply(B)));
 }
Exemple #10
0
 public void SetTransformation(TransformMatrix transform, int positionIndex)
 {
     _transformMatrices[positionIndex] = transform;
 }
Exemple #11
0
        public static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia, TransformMatrix BoneRelMotion)
        {
            Posture post = new Posture();

            TransformMatrix AnatIner = ACS.Inverse() * BoneRelMotion * Inertia;

            double[] myInertia = { 0, 0, 0 };
            for (int i = 0; i < 3; i++)
            {
                myInertia[i] = AnatIner.Array[i][POS_AXIS]; //select which axis to use....
            }
            double x = PROJ_PLANES_SIGN[0] * myInertia[PROJ_PLANES[0]];
            double y = PROJ_PLANES_SIGN[1] * myInertia[PROJ_PLANES[1]];
            double z = PROJ_PLANES_SIGN[2] * myInertia[PROJ_PLANES[2]];
            CartesianCoordinate coord = cart2spherical(x, y, z);
            double theta = coord.az;
            double phi   = coord.elev;

            //if (PROJ_PLANES == {1, 3, -2}...
            theta = (theta / Math.Abs(theta)) * Math.PI - theta;
            //endif

            post.FE_Raw = theta * 180 / Math.PI;
            post.RU_Raw = phi * 180 / Math.PI;

            post.FE = post.FE_Raw - NEUTRAL_FE_POSITION;
            post.RU = post.RU_Raw - NEUTRAL_RU_POSITION;
            return(post);
        }
Exemple #12
0
        public static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia, TransformMatrix MotionRelativeBone, TransformMatrix MotionTestBone)
        {
            TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone;

            return(CalculatePosture(ACS, Inertia, relativeMotion));
        }
Exemple #13
0
 public static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia)
 {
     return(CalculatePosture(ACS, Inertia, new TransformMatrix()));
 }
Exemple #14
0
 public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS)
 {
     return(CalculatePronationSupination(RCS, UCS, new TransformMatrix()));
 }
Exemple #15
0
        public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS, TransformMatrix MotionTestBone, TransformMatrix MotionRelativeBone)
        {
            TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone;

            return(CalculatePronationSupination(RCS, UCS, relativeMotion));
        }
Exemple #16
0
        private TransformMatrix readSingleTransform(StreamReader filestream)
        {
            const string centerRotationRegex  = @"^\s*center\ of\ rotation\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
            const string rotationAnglesRegex  = @"^\s*rotation\ angles\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
            const string translationRegex     = @"^\s*translation\ value\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
            const string translationBothRegex = @"^\s*translation\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";

            //lets figure out the transform type
            TransformMatrix rt            = new TransformMatrix();
            string          transformLine = filestream.ReadLine();
            string          transformType = Regex.Match(transformLine, @"^Transform\ type\:\ (.*)$").Groups[1].Value.Trim();

            switch (transformType)
            {
            case "default rotation":
                _lastTransformWasOptimizedBoth = false;
                double[] centerRotation = new double[3];
                double[] rotationAngles = new double[3];

                string centRotationLine = filestream.ReadLine();
                Match  m = Regex.Match(centRotationLine, centerRotationRegex);
                centerRotation[0] = Double.Parse(m.Groups[1].Value);
                centerRotation[1] = Double.Parse(m.Groups[2].Value);
                centerRotation[2] = Double.Parse(m.Groups[3].Value);

                string rotationAnglesLines = filestream.ReadLine();
                m = Regex.Match(rotationAnglesLines, rotationAnglesRegex);
                rotationAngles[0] = Double.Parse(m.Groups[1].Value);
                rotationAngles[1] = Double.Parse(m.Groups[2].Value);
                rotationAngles[2] = Double.Parse(m.Groups[3].Value);
                rt.rotateAboutCenter(rotationAngles, centerRotation);
                break;

            case "default translation":
                _lastTransformWasOptimizedBoth = false;
                double[] translation = new double[3];

                string translationLine = filestream.ReadLine();
                m = Regex.Match(translationLine, translationRegex);
                translation[0] = Double.Parse(m.Groups[1].Value);
                translation[1] = Double.Parse(m.Groups[2].Value);
                translation[2] = Double.Parse(m.Groups[3].Value);
                rt.setTranslation(translation);
                break;

            case "optimized both":
                _lastTransformWasOptimizedBoth = true;
                centerRotation = new double[3];
                rotationAngles = new double[3];
                translation    = new double[3];

                centRotationLine = filestream.ReadLine();
                m = Regex.Match(centRotationLine, centerRotationRegex);
                centerRotation[0] = Double.Parse(m.Groups[1].Value);
                centerRotation[1] = Double.Parse(m.Groups[2].Value);
                centerRotation[2] = Double.Parse(m.Groups[3].Value);

                rotationAnglesLines = filestream.ReadLine();
                m = Regex.Match(rotationAnglesLines, rotationAnglesRegex);
                rotationAngles[0] = Double.Parse(m.Groups[1].Value);
                rotationAngles[1] = Double.Parse(m.Groups[2].Value);
                rotationAngles[2] = Double.Parse(m.Groups[3].Value);

                translationLine = filestream.ReadLine();
                m = Regex.Match(translationLine, translationBothRegex);
                translation[0] = Double.Parse(m.Groups[1].Value);
                translation[1] = Double.Parse(m.Groups[2].Value);
                translation[2] = Double.Parse(m.Groups[3].Value);
                TransformMatrix rot = new TransformMatrix();
                TransformMatrix t   = new TransformMatrix();
                rot.rotateAboutCenter(rotationAngles, centerRotation);
                t.setTranslation(translation);
                rt = t * rot;
                _currentOptimizedBothTransform = new EuclideanTransform();
                _currentOptimizedBothTransform.CenterRotation = centerRotation;
                _currentOptimizedBothTransform.Rotation       = rotationAngles;
                _currentOptimizedBothTransform.Translation    = translation;
                break;

            default:
                //error, there should be no other type
                throw new FormatException("Invalid format for transform file. Unknown transform type: " + transformType);
            }
            return(rt);
        }