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