private float[,] GetJacobianTranspose()
{
//Matrix rules: num of columns in one has to equal num of rows in the other
//måste endra detta för min matris
//one line for each joint
//forward right left för varje joint
//tre per joint
//ta ut alla nummer per joint
List <Vector3> tempCrosses = new List <Vector3>();
for (int i = 0; i < rigidbodyList.Count - 1; i++)
{ //YZ
Transform body1 = rigidbodyList[i].transform;
Transform endEffector = rigidbodyList.Last().transform;
tempCrosses.Add(Vector3.Cross(body1.right, (endEffector.position - body1.position)));
tempCrosses.Add(Vector3.Cross(body1.up, (endEffector.position - body1.position)));
tempCrosses.Add(Vector3.Cross(body1.forward, (endEffector.position - body1.position)));
}
float[,] matrix = new float[3, tempCrosses.Count];
matrix = MatrixTools.PopulateMatrix(matrix, tempCrosses.ToArray());
return(MatrixTools.TransposeMatrix(matrix));
}
private float[,] GetJacobianTranspose()
{
//Matrix rules: num of columns in one has to equal num of rows in the other
List <Vector3> tempCrosses = new List <Vector3>();
for (int i = 0; i < pairs.Length; i++)
{ //YZ
Transform body1 = pairs[i].musclesYZ[0].rb1.transform;
Transform body2 = pairs[i].musclesYZ[0].rb2.transform;
Transform endEffector = rigidbodyList.Last().transform;
tempCrosses.Add(Vector3.Cross(body1.right, (endEffector.position - body1.position)));
tempCrosses.Add(Vector3.Cross(body1.up, (endEffector.position - body1.position)));
tempCrosses.Add(Vector3.Cross(body1.forward, (endEffector.position - body1.position)));
}
float[,] matrix = new float[3, tempCrosses.Count];
matrix = MatrixTools.PopulateMatrix(matrix, tempCrosses.ToArray());
return(MatrixTools.TransposeMatrix(matrix));
}
