private double lineLength(Vector l)
{
return Math.Sqrt(dot(l, l));
}
private Vector normalize(Vector v)
{
double[] temp = new double[3];
temp[0] = v.x / lineLength(v);
temp[1] = v.y / lineLength(v);
temp[2] = v.z / lineLength(v);
return new Vector(temp);
}
private Vector line(Vector P1, Vector P2)
{
double[] temp = new double[3];
temp[0] = P2.x - P1.x;
temp[1] = P2.y - P1.y;
temp[2] = P2.z - P1.z;
return new Vector(temp);
}
private double dot(Vector P1, Vector P2)
{
return (P1.x * P2.x) + (P1.y * P2.y) + (P1.z * P2.z);
}
private Vector cross(Vector P1, Vector P2)
{
double[] temp = new double[3];
temp[0] = (P1.y * P2.z) - (P1.z * P2.y);
temp[1] = (P1.z * P2.x) - (P1.x * P2.z);
temp[2] = (P1.x * P2.y) - (P1.y * P2.x);
return new Vector(temp);
}
public double getYOnScreen(SkeletonPoint p)
{
double distToPoint, theta;
Vector pointOnPlane, lineToPoint;
double[] temp = new double[3];
temp[0] = p.X - (dot(normalVector, convert(p)) + d) * normalVector.x;
temp[1] = p.Y - (dot(normalVector, convert(p)) + d) * normalVector.y;
temp[2] = p.Z - (dot(normalVector, convert(p)) + d) * normalVector.z;
pointOnPlane = new Vector(temp);
lineToPoint = line(point, pointOnPlane);
distToPoint = lineLength(lineToPoint);
theta = Math.Acos(dot(lineToPoint, line1) / (distToPoint * height));
return (distToPoint * Math.Cos(theta)) / height;
}
//line1 is the line from the top-left corner to bottom-left corner
//line2 is the line from the top-left corner to top-right corner
//normalVector is a unit vector that is perpendicular to the screen
//point is the top-left corner of the screen
//d is a constant needed for the plane equation
//height is the height of the screen in meters
//width is the width of the screen in meters
public myScreen(SkeletonPoint P1, SkeletonPoint P2, SkeletonPoint P3)
{
line1 = line(convert(P1), convert(P2));
line2 = line(convert(P1), convert(P3));
normalVector = cross(line1, line2);
normalVector = normalize(normalVector);
point = convert(P1);
d = -(dot(normalVector, point));
height = lineLength(line1);
width = lineLength(line2);
}
