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