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