// Use this for initialization
void Start()
{
velocityP = GameObject.Find("Pelota").GetComponent <BallPhysics>().lVelocityInit;
gravity = GameObject.Find("Pelota").GetComponent <BallPhysics>().fGravity;
Welocity = GameObject.Find("Pelota").GetComponent <BallPhysics>().wVelocity;
tau = GameObject.Find("Pelota").GetComponent <BallPhysics>().fTau;
}
// Use this for initialization
void Start()
{
scriptToAccess = Pelota.GetComponent <BallPhysics>();
if (VariableToGet == 0)
{
LINE_lVelocityFin = scriptToAccess.lVelocityFin;
transform.position = new Vector3(LINE_lVelocityFin.x, LINE_lVelocityFin.y, LINE_lVelocityFin.z);
}
if (VariableToGet == 1)
{
LINE_lVelocityFin = scriptToAccess.fMagnus;
transform.position = new Vector3(LINE_fMagnus.x, LINE_fMagnus.y, LINE_fMagnus.z);
}
if (VariableToGet == 2)
{
LINE_fDrag = scriptToAccess.fDrag;
transform.position = new Vector3(LINE_fDrag.x, LINE_fDrag.y, LINE_fDrag.z);
}
if (VariableToGet == 3)
{
LINE_fTau = scriptToAccess.fTau;
transform.position = new Vector3(LINE_fTau.x, LINE_fTau.y, LINE_fTau.z);
}
}
// Update is called once per frame
void Update()
{
if (VariableToGet == 0)
{
LINE_lVelocityFin = scriptToAccess.lVelocityFin;
transform.position = new Vector3(LINE_lVelocityFin.x, LINE_lVelocityFin.y, LINE_lVelocityFin.z);
}
if (VariableToGet == 1)
{
LINE_fMagnus = scriptToAccess.fMagnus;
transform.position = new Vector3(LINE_fMagnus.x, LINE_fMagnus.y, LINE_fMagnus.z);
}
if (VariableToGet == 2)
{
LINE_fDrag = scriptToAccess.fDrag;
transform.position = new Vector3(LINE_fDrag.x, LINE_fDrag.y, LINE_fDrag.z);
}
if (VariableToGet == 3)
{
LINE_fTau = scriptToAccess.fTau;
transform.position = new Vector3(LINE_fTau.x, LINE_fTau.y, LINE_fTau.z);
}
}
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;
}
public void Add(Our_Vector3 vector_b) //El resultado sobreescribe el vector sobre el que se hace el .Add
{
x = x + vector_b.x;
y = y + vector_b.y;
z = z + vector_b.z;
//return new Our_Vector3(x, y, z);
}
public Our_Quaternion(float angle, Our_Vector3 axis) //Al quaternion hay que pasarle el eje que obtenemos a hacer click en la pelota + el angulo de giro que queremos
{
axis.Normalize();
w = Mathf.Cos((angle * Mathf.Deg2Rad) / 2);
x = axis.x * Mathf.Sin((angle * Mathf.Deg2Rad) / 2);
y = axis.y * Mathf.Sin((angle * Mathf.Deg2Rad) / 2);
z = axis.z * Mathf.Sin((angle * Mathf.Deg2Rad) / 2);
}
void Start()
{
getKickPosition = GameObject.Find("ScriptsObject").GetComponent <GetKickPosition>().fromBallCoordinates; //Punto de impacto a la pelota respecto a su centro
airDensity = 1.23f;
Cd = 0.25f; //0.25 originalmente
Cm = 0.25f;
mass = 0.396f; //0.396f
//radius = 0.279f; //0.279f
dt = 0.01f; //PUEDE SER Q NECESITEMOS 2 DT
inertiaMoment = (0.667f) * mass * (radius * radius);
Kd = 0.5f * airDensity * Cd * Area();
Km = 0.5f * airDensity * Cm * Area();
}
void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
startKick();
startKicked = true;
}
//APLICAMOS LA ROTACION A LA PELOTA A PARTIR DE FTAU
transform.Rotate(new Vector3(fTau.x, fTau.y, fTau.z), wVelocity.Module()); //PARA ROTAR PASAMOS EL EJE DE ROTACION Y EL MODULO DE HOMEGA
Debug.DrawLine(new Vector3(transform.position.x, transform.position.y, transform.position.z), new Vector3(VectorDireccion.position.x, VectorDireccion.position.y, VectorDireccion.position.z), Color.black); //VEC DIRECCION IMPACTO
Debug.DrawRay(transform.position, new Vector3(fTau.x, fTau.y, fTau.z), Color.black); //VECTOR DIR HOMEGA, EJE DE ROTACION
if (startKicked == true)
{
//Calcular fDrag
fDrag.x = -Kd *lVelocityInit.Module() * lVelocityInit.x;
fDrag.y = -Kd *lVelocityInit.Module() * lVelocityInit.y;
fDrag.z = -Kd *lVelocityInit.Module() * lVelocityInit.z;
//Calcular fMagnus
Our_Vector3 wVelocityAux = new Our_Vector3(wVelocity.x, wVelocity.y, wVelocity.z);
wVelocityAux.Normalize();
fMagnus.x = Km * lVelocityInit.Module() * (wVelocityAux.y * lVelocityInit.z - lVelocityInit.y * wVelocityAux.z);
fMagnus.y = Km * lVelocityInit.Module() * (wVelocityAux.x * lVelocityInit.z - lVelocityInit.x * wVelocityAux.z);
fMagnus.z = Km * lVelocityInit.Module() * (wVelocityAux.x * lVelocityInit.y - lVelocityInit.x * wVelocityAux.y);
//Agrupar fTotal
fTotal.x = fDrag.x + fMagnus.x + fGravity.x;
fTotal.y = fDrag.y + fMagnus.y + fGravity.y;
fTotal.z = fDrag.z + fMagnus.z + fGravity.z;
//ACTUALIZAR lVelocityInit A PARTIR DE LAS NUEVAS FORUMLAS CON EL METODO DE EULER
float aTx = fTotal.x / mass;
float aTy = fTotal.y / mass;
float aTz = fTotal.z / mass;
//Debug.Log("Antes" + lVelocityInit.Module());
lVelocityFin = lVelocityInit;
lVelocityInit.x = lVelocityFin.x + aTx * Time.deltaTime;
lVelocityInit.y = lVelocityFin.y + aTy * Time.deltaTime;
lVelocityInit.z = lVelocityFin.z + aTz * Time.deltaTime;
transform.position = new Vector3(transform.position.x, transform.position.y, transform.position.z)
+ new Vector3(lVelocityFin.x, lVelocityFin.y, lVelocityFin.z) * Time.deltaTime;
}
}
void Update()
{
if (Input.GetMouseButtonDown(0))
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out colision))
{
newPelota.transform.position = colision.point;
//Coordenadas del click respecto a la pelota.
fromBallCoordinates.x = newPelota.transform.position.x - colision.transform.position.x;
fromBallCoordinates.y = newPelota.transform.position.y - colision.transform.position.y;
fromBallCoordinates.z = newPelota.transform.position.z - colision.transform.position.z;
}
}
if (Input.GetKey(KeyCode.UpArrow))
{
Our_Vector3 rot = new Our_Vector3(0, 0.01f, 0);
VectorDireccion.transform.Translate(rot);
}
if (Input.GetKey(KeyCode.DownArrow))
{
Our_Vector3 rot = new Our_Vector3(0, -0.01f, 0);
VectorDireccion.transform.Translate(rot);
}
if (Input.GetKey(KeyCode.LeftArrow))
{
Our_Vector3 rot = new Our_Vector3(0, 0, -0.01f);
VectorDireccion.transform.Translate(rot);
}
if (Input.GetKey(KeyCode.RightArrow))
{
Our_Vector3 rot = new Our_Vector3(0, 0, 0.01f);
VectorDireccion.transform.Translate(rot);
}
}
// Update is called once per frame
void Update()
{
// 0.5 --- 3
Our_Vector3 dist = new Our_Vector3(pelota.transform.position.x - transform.position.x, pelota.transform.position.y - transform.position.y, pelota.transform.position.z - transform.position.z);
Debug.DrawLine(pelota.transform.position, transform.position, Color.green);
if (dist.Module() < 0.2)
{
//Debug.Log("Col");
gravity.y = 0;
Welocity.module = 0;
Welocity.x = 0;
Welocity.y = 0;
Welocity.z = 0;
tau.x = 0;
tau.y = 0;
tau.z = 0;
velocityP.x = 0;
velocityP.y = 0;
velocityP.z = 0;
}
//Debug.Log(velocityP.x);
}
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;
}
}
public void Substract(Our_Vector3 vector_b) //El resultado sobreescribe el vector sobre el que se hace el .Substract
{
x = x - vector_b.x;
y = y - vector_b.y;
z = z - vector_b.z;
}
public float DotProduct(Our_Vector3 vector_b)
{
return((x * vector_b.x) + (y * vector_b.y) + (z * vector_b.z));
}
public Our_Vector3 CrossProduct(Our_Vector3 vector_b) //Devuelve un vector nuevo sin sobreescribir ningun resultado
{
Our_Vector3 res = new Our_Vector3(y * vector_b.z - z * vector_b.y, z * vector_b.x - x * vector_b.z, x * vector_b.y - y * vector_b.x);
return(res);
}
