Пример #1
0
        public double[] transformMoments(double[] forces)
        {
            double[] fforce = new double[6];
            forces.CopyTo(fforce, 0);
            DenseVector forceV = new DenseVector(fforce);
            DenseVector tforce = (DenseVector)forceV.SubVector(0, 3);
            DenseVector moments = (DenseVector)forceV.SubVector(3, 3);

            DenseVectorCrossProduct crs = new DenseVectorCrossProduct(tforce);
            moments = (DenseVector)crs.crossProduct(directionalVector).Add(moments);

            forceV.SetSubVector(3, 3, moments);
            return forceV.Values;
        }
Пример #2
0
        private void actuatorForce2Cartesian()
        {
            DenseVector force = new DenseVector(3);
            DenseVector moment = new DenseVector(3);
            DenseVector translation = new DenseVector(3);
            cartesianDisp.CopySubVectorTo(translation, 0, 0, 3);

            for (int a = 0; a < 6; a++)
            {
                DenseVector unitVector = (DenseVector)actuators[a].getDirectionalVector();
                unitVector = (DenseVector)unitVector.Normalize(2.0);
                DenseVector forceArm = actuators[a].getForceArm(translation);
                DenseVector newForce = (DenseVector)unitVector.Multiply(actuatorForce[a]);
                force = (DenseVector)force.Add(newForce);
                DenseVectorCrossProduct cx = new DenseVectorCrossProduct(forceArm);
                DenseVector newMoment = (DenseVector)cx.crossProduct(unitVector);
                newMoment = (DenseVector)newMoment.Multiply(actuatorForce[a]);
                moment = (DenseVector)moment.Add(newMoment);
            }
            cartesianForce.SetSubVector(0, 3, force);
            cartesianForce.SetSubVector(3, 3, moment);
        }