public TransformMatrix CalculateRelativeMotion(int startPositionIndex, int endPositionIndex, Bone fixedBone) { TransformMatrix startRelTransform = this.CalculateRelativeMotionFromNeutral(startPositionIndex, fixedBone); TransformMatrix endRelTransform = this.CalculateRelativeMotionFromNeutral(endPositionIndex, fixedBone); return(endRelTransform * startRelTransform.Inverse()); }
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); }
public void CalculateAndSaveDistanceMapForAnimation(int[] animationOrder, int numFramesPerStep, int absoluteFrameNumber, Bone[] testBones, Bone fixedBone) { //quick check that we can do this. Don't need our own distance field, just the testBones' if (_coloredBone == null) { return; } //need to calculate where we are in the whole animation scheme. What two positions are we between? int startPositionIndex = absoluteFrameNumber / numFramesPerStep; int partialFrame = absoluteFrameNumber % numFramesPerStep; int startPosition = animationOrder[startPositionIndex]; //calculate the relative motion for each bone. Need a transform for each testBone, that will //move this bone into its coordinate system TransformMatrix[] transforms = new TransformMatrix[testBones.Length]; if (partialFrame == 0) //no partial frame, we are exactly where we want to be { if (startPosition != 0) //if we are at absolute neutral, no calculations needed { //check for the non for (int i = 0; i < testBones.Length; i++) { transforms[i] = CalculateRelativeMotionFromNeutral(startPosition, testBones[i]); } } } else { //so we have a partial motion to deal with, first calculate the position of the current bone TransformMatrix tmCurrentBone = CalculateInterpolatedMotion(startPosition, animationOrder[startPositionIndex + 1], fixedBone, numFramesPerStep)[partialFrame]; for (int i = 0; i < testBones.Length; i++) { TransformMatrix tmTestBone = testBones[i].CalculateInterpolatedMotion(startPosition, animationOrder[startPositionIndex + 1], fixedBone, numFramesPerStep)[partialFrame]; transforms[i] = tmTestBone.Inverse() * tmCurrentBone; } } double[] distances = DistanceMaps.createDistanceMap(this, testBones, transforms); lock (this) { if (_animationComputedDistances == null) { _animationComputedDistances = new double[(animationOrder.Length - 1) * numFramesPerStep + 1][]; } _animationComputedDistances[absoluteFrameNumber] = distances; } }
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); }
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); }
public static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia, TransformMatrix MotionRelativeBone, TransformMatrix MotionTestBone) { TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone; return(CalculatePosture(ACS, Inertia, relativeMotion)); }
public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS, TransformMatrix MotionTestBone, TransformMatrix MotionRelativeBone) { TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone; return(CalculatePronationSupination(RCS, UCS, relativeMotion)); }