public static double AnguloRectang(float segH, float segA)
{
double rad = Math.Acos(segA / segH);
double deg = DistanceHelper.RadToDeg(rad);
return(deg);
}
//Método calculo ángulo donde:
public static double CalcularAngulo(float segmentoA, float segmentoB, float segmentoC)
{
//se devuelve el ángulo del primer punto, es decir que si el hombro es "a",
//devuelve el angulo del hombro en relación al codo y hombro.
float segA2 = (float)Math.Pow(segmentoA, 2);
float segB2 = (float)Math.Pow(segmentoB, 2);
float segC2 = (float)Math.Pow(segmentoC, 2);
double rad = Math.Acos((segA2 - segB2 - segC2) / ((-2) * segmentoB * segmentoC));
double deg = DistanceHelper.RadToDeg(rad);
return(deg);
//return rad;
}
public static double[] SetValorAngulos(Joint hombro, Joint mano, Joint codo, SkeletonPoint skelObjeto)
{
double[] array = new double[4];
array[0] = -1;
array[1] = -1;
array[2] = -1;
array[3] = -1;
//punto auxiliar en el torso para calcular el ángulo del hombro
//en el movimiento arriba - abajo.
SkeletonPoint puntoTorso = new SkeletonPoint();
puntoTorso.X = hombro.Position.X;
puntoTorso.Y = codo.Position.Y;
puntoTorso.Z = hombro.Position.Z;
//calculamos la distancia entre el objeto y el hombro.
float distHombroObj = DistanceHelper.ObtenerDistancia(hombro, skelObjeto);
//calculamos la distancia entre las articulaciones:
//distancia entre hombro y mano
float distHombroMano = DistanceHelper.ObtenerDistancia(mano, hombro);
//distancia entre mano y codo
float distCodoMano = DistanceHelper.ObtenerDistancia(mano, codo);
//distancia entre codo y hombro
float distHombroCodo = DistanceHelper.ObtenerDistancia(codo, hombro);
//distancia entre hombro y punto auxiliar del torso
float distTorsoHombro = DistanceHelper.ObtenerDistancia(hombro, puntoTorso);
//distancia entre mano y objeto
float distManoObj = DistanceHelper.ObtenerDistancia(mano, skelObjeto);
Console.WriteLine($"Distancia Codo mano {distCodoMano}");
Console.WriteLine($"Distancia Hombro Codo {distHombroCodo}");
Console.WriteLine($"Distancia Hombro Objeto {distHombroObj}");
if (distHombroObj > (distCodoMano + distHombroCodo))
{
Console.WriteLine("no se puede alcanzar el objeto!");
}
else
{
//el primer argumento es el segmento opuesto al angulo que queremos obtener.
double anguloHombroAdelanteAtrasFut = DistanceHelper.CalcularAngulo(distManoObj,
distHombroObj, distHombroMano);
double anguloCodoFut = DistanceHelper.CalcularAngulo(distHombroObj,
distHombroCodo, distCodoMano);
float distAuxManoObjeto = (mano.Position.Y - skelObjeto.Y);
puntoTorso.Y = codo.Position.Y + distAuxManoObjeto;
double anguloHombroArribaAbajoFut = DistanceHelper.AnguloRectang(distHombroCodo, distTorsoHombro);
//si el punto del codo está más arriba que el hombro, el angulo se calculará para el otro lado
//entonces hay que sacar el suplementario para que corresponda al servo.
if (puntoTorso.Y > hombro.Position.Y)
{
anguloHombroArribaAbajoFut = 180 - anguloHombroArribaAbajoFut;
}
if (anguloHombroAdelanteAtrasFut < 90 && anguloHombroAdelanteAtrasFut > 0 &&
anguloCodoFut < 90 && anguloCodoFut > 0)
{
array[0] = anguloCodoFut;
array[1] = 090;
array[2] = anguloHombroAdelanteAtrasFut;
array[3] = anguloHombroArribaAbajoFut;
}
}
return(array);
}