public static double AngleBetweenJoints(MyJoint j1, MyJoint j2, MyJoint j3)
{
double Angulo = 0;
double shrhX = j1.getX() - j2.getX();
double shrhY = j1.getY() - j2.getY();
double shrhZ = j1.getZ() - j2.getZ();
double hsl = vectorNorm(shrhX, shrhY, shrhZ);
double unrhX = j3.getX() - j2.getX();
double unrhY = j3.getY() - j2.getY();
double unrhZ = j3.getZ() - j2.getZ();
double hul = vectorNorm(unrhX, unrhY, unrhZ);
double mhshu = shrhX * unrhX + shrhY * unrhY + shrhZ * unrhZ;
double x = mhshu / (hul * hsl);
if (x != Double.NaN)
{
if (-1 <= x && x <= 1)
{
double angleRad = Math.Acos(x);
Angulo = angleRad * (180.0 / Math.PI);
}
else
{
Angulo = 0;
}
}
else
{
Angulo = 0;
}
return(Angulo);
}
public MyAngle()
{
_myJoint = new MyJoint[25];
angle = new int[25];
for (int i = 0; i < 25; i++)
{
_myJoint[i] = new MyJoint();
}
}