示例#1
0
 public double[] transform(double[] displacement, bool isreverse)
 {
     DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
     double[] dispcopy = new double[6];
     displacement.CopyTo(dispcopy, 0);
     DenseMatrix2String m2s = new DenseMatrix2String();
     List2String l2s = new List2String();
     DenseVector dispV = new DenseVector(dispcopy);
     log.Debug("original disp: " + l2s.ToString(displacement));
     if (isreverse)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
         log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
     }
     DenseVector newDisp = translate(dispV.Values, isreverse);
     log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
     DenseMatrix rollM = roll.create(dispV.Values[3]);
     DenseMatrix pitchM = pitch.create(dispV.Values[4]);
     DenseMatrix yawM = yaw.create(dispV.Values[5]);
     DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
     log.Debug("rotation: " + m2s.ToString(rotation));
     DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
     log.Debug("unt: " + l2s.ToString(unt.Values));
     DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
     log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
     dispV.SetSubVector(0, 3, newDisp1);
     if (isreverse == false)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
     }
     log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
     return dispV.Values;
 }
        public void setLbcb(bool isLbcb2, double[] flattenedPinArray, double[] motionCenter, double[] flattenedTransformation)
        {
            double[][] pins = new double[6][];
            double[,] xform = new double[6, 6];
            double[,] xpins = new double[6, 6];
            List2String l2s = new List2String();
            DenseMatrix2String m2s = new DenseMatrix2String();

            for (int r = 0; r < 6; r++)
            {
                pins[r] = new double[6];
                for (int c = 0; c < 6; c++)
                {
                    pins[r][c] = flattenedPinArray[r * 6 + c];
                    xpins[r,c] = flattenedPinArray[r * 6 + c];
                    xform[r, c] = flattenedTransformation[r * 6 + c];

                }
            }

            log.Info("Creating " + (isLbcb2 ? "LBCB 2" : "LBCB 1")
                + " with \n]\t Motion Center: " + l2s.ToString(motionCenter)
               + "\n\t Transformation Matrix: " + m2s.ToString(DenseMatrix.OfArray(xform))
               + "\n\t Pins Locations: " + m2s.ToString(DenseMatrix.OfArray(xpins)));
               Lbcb lbcb = new Lbcb((isLbcb2 ? "LBCB 2" : "LBCB 1"), pins);
            lbcbs[(isLbcb2 ? 1 : 0)] = lbcb;
            RigidTransform transform = new RigidTransform(motionCenter,xform);
            transforms[(isLbcb2 ? 1 : 0)] = transform;
        }