private float[] GetDeltaOrientation() { //varje index skillnaden för varje joint float[,] Jt = GetJacobianTranspose(); Vector3 V = (target.transform.position - rigidbodyList.Last().transform.position); Debug.Log("LAST: " + joints[2]); //Vector3 V = new Vector3(GetCoMHit().x, 0.0f, GetCoMHit().z) - new Vector3(target.x, 0.0f, target.z); //multiplicera min jacobian tyranspose med v //dO = Jt * V; float[,] dO = MatrixTools.MultiplyMatrix(Jt, new float[, ] { { V.x }, { V.y }, { V.z } }); //varje float är en kolumn i matrisen //varför förasta vädet i varje kolumn? //testa ta ut alla värden? List <float> angles = new List <float>(); for (int i = 0; i < (rigidbodyList.Count - 1) * 3; i++) { angles.Add(dO[i, 0]); } return(angles.ToArray()); }
private float[] GetDeltaOrientation() { float[,] Jt = GetJacobianTranspose(); Vector3 V = (target.transform.position - rigidbodyList.Last().transform.position); //dO = Jt * V; float[,] dO = MatrixTools.MultiplyMatrix(Jt, new float[, ] { { V.x }, { V.y }, { V.z } }); List <float> jointAngles = new List <float>(); for (int i = 0; i < pairs.Length * 3; i++) { jointAngles.Add(dO[i, 0]); } return(jointAngles.ToArray()); //return new float[] { dO[0, 0], dO[1,0], dO[2,0] }; }