public void Multiply(Our_Quaternion b)
{
float tempW, tempX, tempY, tempZ;
tempW = (w * b.w) - (x * b.x) - (y * b.y) - (z * b.z); // w
tempX = (w * b.x) + (x * b.w) - (y * b.z) + (z * b.y); // x
tempY = (w * b.y) + (x * b.y) + (y * b.w) - (z * b.x); // y
tempZ = (w * b.z) - (x * b.y) + (y * b.x) + (z * b.w); // z
w = tempW;
x = tempX;
y = tempY;
z = tempZ;
}
void Update()
{
if (!done)
{
if (intentos <= M_intentos)
{
for (int i = joints.Length - 1; i >= 0; i--) //AQUI ESTABA -2 !!!
{
Our_Vector3 r1 = new Our_Vector3(0, 0, 0);
r1.x = joints[joints.Length - 1].transform.position.x - joints[i].transform.position.x;
r1.y = joints[joints.Length - 1].transform.position.y - joints[i].transform.position.y;
r1.z = joints[joints.Length - 1].transform.position.z - joints[i].transform.position.z;
Our_Vector3 r2 = new Our_Vector3(0, 0, 0);
r2.x = targetPosition.x - joints[i].transform.position.x;
r2.y = targetPosition.y - joints[i].transform.position.y;
r2.z = targetPosition.z - joints[i].transform.position.z;
if (r1.Module() * r2.Module() <= 0.001f)
{
}
else
{
cos[i] = r1.DotProduct(r2) / (r1.Module() * r2.Module());
sin[i] = r1.CrossProduct(r2).Module() / (r1.Module() * r2.Module());
}
Our_Vector3 rotationAxis = rotationAxis = r1.CrossProduct(r2);
rotationAxis.Normalize();
if (type == "cadera")
{
if (i == 0)
{
theta[i] = Mathf.Acos(cos[i]);
theta[i] = theta[i] * Mathf.Rad2Deg;
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
Our_Quaternion rt = new Our_Quaternion(joints[i].transform.rotation.x, joints[i].transform.rotation.y, joints[i].transform.rotation.z, joints[i].transform.rotation.w);
Our_Quaternion myRotation = new Our_Quaternion(theta[i], rotationAxis); //ESTO FALLA
myRotation.Multiply(rt);
myRotation.y = myRotation.z = 0; //Rotation only in X axis.
Quaternion temp = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
float angleF;
Vector3 axisF;
temp.ToAngleAxis(out angleF, out axisF);
if (angleF > 130 && angleF < 230)
{
joints[i].transform.rotation = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
}
}
}
if (type == "brazoD")
{
if (i == 0)
{
theta[i] = Mathf.Acos(cos[i]);
theta[i] = theta[i] * Mathf.Rad2Deg;
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
Our_Quaternion rt = new Our_Quaternion(joints[i].transform.rotation.x, joints[i].transform.rotation.y, joints[i].transform.rotation.z, joints[i].transform.rotation.w);
Our_Quaternion myRotation = new Our_Quaternion(theta[i], rotationAxis); //ESTO FALLA
myRotation.Multiply(rt);
myRotation.y = myRotation.z = 0; //Rotation only in X axis.
Quaternion temp = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
float angleF;
Vector3 axisF;
temp.ToAngleAxis(out angleF, out axisF);
if (angleF > 15 && angleF < 270)
{
joints[i].transform.rotation = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
}
}
if (i == 1)
{
theta[i] = Mathf.Acos(cos[i]);
theta[i] = theta[i] * Mathf.Rad2Deg;
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
Our_Quaternion rt = new Our_Quaternion(joints[i].transform.rotation.x, joints[i].transform.rotation.y, joints[i].transform.rotation.z, joints[i].transform.rotation.w);
Our_Quaternion myRotation = new Our_Quaternion(theta[i], rotationAxis); //ESTO FALLA
myRotation.Multiply(rt);
myRotation.y = myRotation.z = 0; //Rotation only in X axis.
Quaternion temp = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
float angleF;
Vector3 axisF;
temp.ToAngleAxis(out angleF, out axisF);
if (angleF > 10 && angleF < 90)
{
joints[i].transform.rotation = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
}
}
}
if (type == "brazoI")
{
if (i == 0)
{
theta[i] = Mathf.Acos(cos[i]);
theta[i] = theta[i] * Mathf.Rad2Deg;
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
Our_Quaternion rt = new Our_Quaternion(joints[i].transform.rotation.x, joints[i].transform.rotation.y, joints[i].transform.rotation.z, joints[i].transform.rotation.w);
Our_Quaternion myRotation = new Our_Quaternion(theta[i], rotationAxis); //ESTO FALLA
myRotation.Multiply(rt);
myRotation.y = myRotation.z = 0; //Rotation only in X axis.
Quaternion temp = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
float angleF;
Vector3 axisF;
temp.ToAngleAxis(out angleF, out axisF);
if (angleF > 90 && angleF < 345)
{
joints[i].transform.rotation = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
}
}
if (i == 1)
{
theta[i] = Mathf.Acos(cos[i]);
theta[i] = theta[i] * Mathf.Rad2Deg;
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
Our_Quaternion rt = new Our_Quaternion(joints[i].transform.rotation.x, joints[i].transform.rotation.y, joints[i].transform.rotation.z, joints[i].transform.rotation.w);
Our_Quaternion myRotation = new Our_Quaternion(theta[i], rotationAxis); //ESTO FALLA
myRotation.Multiply(rt);
myRotation.y = myRotation.z = 0; //Rotation only in X axis.
Quaternion temp = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
float angleF;
Vector3 axisF;
temp.ToAngleAxis(out angleF, out axisF);
if (angleF > 50 && angleF > 270)
{
joints[i].transform.rotation = new Quaternion(myRotation.x, myRotation.y, myRotation.z, myRotation.w);
}
}
}
}
intentos++;
}
}
float f = (targetPosition.x - joints[joints.Length - 1].transform.position.x + targetPosition.y - joints[joints.Length - 1].transform.position.y + targetPosition.z - joints[joints.Length - 1].transform.position.z);
if (f < rango)
{
done = true;
}
else
{
done = false;
} //End effector esta lo suficientemente cerca del target
if (targetPosition.x != target.transform.position.x || targetPosition.y != target.transform.position.y || targetPosition.z != target.transform.position.z) //targetPosition != target.transform.position
{
intentos = 0;
targetPosition.x = target.transform.position.x;
targetPosition.y = target.transform.position.y;
targetPosition.z = target.transform.position.z;
}
}
