void startKick() { //LineRenderer myLine = gameObject.AddComponent<LineRenderer>(); fGravity = new Our_Vector3(0, -mass * gravity, 0); fMagnus = new Our_Vector3(0, 0, 0); fDrag = new Our_Vector3(0, 0, 0); fTau = new Our_Vector3(0, 0, 0); rad.x = getKickPosition.x - transform.position.x; rad.y = getKickPosition.y - transform.position.y; rad.z = getKickPosition.z - transform.position.z; //fP.module = barra.value;// FUERZA TOTAL DE FP dirfP.x = (getKickPosition.x - VectorDireccion.position.x); dirfP.y = (getKickPosition.y - VectorDireccion.position.y); dirfP.z = (getKickPosition.z - VectorDireccion.position.z); //FP EN SUS COMPONENTES dirfP.Normalize(); fP.x = dirfP.x * barra.value; fP.y = dirfP.y * barra.value; fP.z = dirfP.z * barra.value; //Calcular direccion Tau fTau = rad.CrossProduct(fP); //Calcular wVelocity (Inicial) wVelocity.x = fTau.x / inertiaMoment * dt; wVelocity.y = fTau.y / inertiaMoment * dt; wVelocity.z = fTau.z / inertiaMoment * dt; //Calcular lVelocity (Inicial) lVelocityInit.x = (fP.x * dt) / mass; lVelocityInit.y = (fP.y * dt) / mass; lVelocityInit.z = (fP.z * dt) / mass; }
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; } }