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; }
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); }