public void TestDotProduct()
{
Vector3D v1 = new Vector3D(1, 2, 7);
Vector3D v2 = new Vector3D(2, 8, 1);
Assert.AreEqual(v1.DotProduct(v2), 25);
Assert.AreEqual(v1*v2, 25);
}
private static double SquaredDist(Vector3D cur, Vector3D old)
{
double dx = cur.X - old.X;
double dy = cur.Y - old.Y;
double dz = cur.Z - old.Z;
return dx*dx + dy*dy + dz*dz;
}
public void TestValidNormalise()
{
Vector3D v1 = new Vector3D(5, 4, 3);
// Magnitude = √(5^2 + 4^2 + 3^2) = √50
// Therefore expected: x = 5/√50, y = 4/√50, z = 3/√50
TestNormalisedExpected(v1, 5 / Math.Sqrt(50), 4 / Math.Sqrt(50), 3 / Math.Sqrt(50));
Vector3D v2 = new Vector3D(1, 6, 7);
// Magnitude = √(1^2 + 6^2 + 7^2) = √86
// Therefore expected: x = 5/√86, y = 4/√86, z = 3/√86
TestNormalisedExpected(v2, 1 / Math.Sqrt(86), 6 / Math.Sqrt(86), 7 / Math.Sqrt(86));
}
public double DotProduct(Vector3D other)
{
return (this.X * other.X) + (this.Y * other.Y) + (this.Z * other.Z);
}
private static Vector3D Clone(Vector3D cur)
{
return new Vector3D(cur.X, cur.Y, cur.Z);
}
public void TestXYZ(Vector3D v, double expectedX, double expectedY, double expectedZ)
{
Assert.AreEqual(expectedX, v.X, Precision);
Assert.AreEqual(expectedY, v.Y, Precision);
Assert.AreEqual(expectedZ, v.Z, Precision);
}
private static Tuple<byte[], BitArray> ProcessHand(Vector3D handPos, Vector3D wristPos, byte[] video, byte[] depth)
{
// convert hand and wrist positions to respective co-ord systems:
int depthHandX, depthHandY, depthWristX, depthWristY, videoX, videoY;
HandCoords(wristPos, out depthWristX, out depthWristY);
HandCoords(handPos, out videoX, out videoY, out depthHandX, out depthHandY);
// extract depth values
return Tuple.Create(CropVideo(video, videoX, videoY),
GetDepthMap(depth, depthHandX, depthHandY, depthWristX, depthWristY));
}
/// <summary>
/// Convert skeleton co-ordinate to depth-space co-ordinates
/// </summary>
/// <param name="handPos">The position of the hand as per the skeleton</param>
/// <param name="depthXf">(output) x-ordinate in [0,1] space</param>
/// <param name="depthYf">(output) y-ordinate in [0,1] space</param>
public void GetDepthCoordsFromSkeleton(Vector3D handPos, out float depthXf, out float depthYf)
{
// defer to the Kinect driver (needs conversion to Kinect Vector format)
var kinectVector = new Vector() {W = 0, X = (float) handPos.X, Y = (float) handPos.Y, Z = (float) handPos.Z};
GetDepthCoordsFromSkeleton(kinectVector, out depthXf, out depthYf);
}
/// <summary>
/// NOTE: This is a special case helper method
///
/// Find the angle between two <b>Unit</b> Vectors.
/// i.e. this means that norm(uva) * norm(uvb) = 1
///
/// Also, uva and uvb must meet either at the heads or at the tails.
/// i.e. If the head of one meets the tail of another,
/// Then the supplementary angle is returned!
/// (This is how the inverse Cosine function behaves, arccos(-x) = PI-arccos(x); )
///
/// Hence, this is a private method.
/// </summary>
private static double Angle(Vector3D uva, Vector3D uvb)
{
return Math.Acos(uva*uvb);
}
public void TestNormalisedExpected(Vector3D v, double expectedX, double expectedY, double expectedZ)
{
v.Normalise();
TestXYZ(v, expectedX, expectedY, expectedZ);
}
public void TestNegate()
{
Vector3D v1 = new Vector3D(1, 2, 7);
TestXYZ(-v1, -1, -2, -7);
}
public void TestInvalidNormalise()
{
Vector3D zeroVector = new Vector3D(0, 0, 0);
zeroVector.Normalise();
TestXYZ(zeroVector, 0, 0, 0);
}
public void TestAdd()
{
Vector3D v1 = new Vector3D(1, 2, 7);
Vector3D v2 = new Vector3D(2, 8, 1);
TestXYZ(v1 + v2, 3, 10, 8);
}
private static void HandCoords(Vector3D handPos, out int depthX, out int depthY)
{
float depthXf, depthYf;
KinectHandler.Get().GetDepthCoordsFromSkeleton(handPos, out depthXf, out depthYf);
// convert to integer co-ordinates (from (1 x 1) space to (width x height)):
depthX = (int)Math.Max(0, Math.Min(depthXf * DepthWidth, DepthWidth));
depthY = (int)Math.Max(0, Math.Min(depthYf * DepthHeight, DepthHeight));
}
private static double[] FeatureAnglesOf(Vector3D[] featureVectors)
{
var result = new double[Enum.GetValues(typeof(Angles)).Length];
result[(int)Angles.AngleShoulderLeft] = Angle(
-featureVectors[(int)DirectionVectors.ShoulderBladeLeft],
featureVectors[(int)DirectionVectors.ArmLeft]);
result[(int)Angles.AngleShoulderRight] = Angle(
-featureVectors[(int)DirectionVectors.ShoulderBladeRight],
featureVectors[(int)DirectionVectors.ArmRight]);
result[(int)Angles.AngleElbowLeft] = Angle(
-featureVectors[(int)DirectionVectors.ArmLeft],
featureVectors[(int)DirectionVectors.ForeArmLeft]);
result[(int)Angles.AngleElbowRight] = Angle(
-featureVectors[(int)DirectionVectors.ArmRight],
featureVectors[(int)DirectionVectors.ForeArmRight]);
return result;
}
private static Vector3D[] FeatureVectorsOf(Skeleton sk)
{
var result = new Vector3D[Enum.GetValues(typeof(DirectionVectors)).Length];
result[(int)DirectionVectors.HandLeft] = DirectionVector(sk.WristLeft, sk.PalmLeft);
result[(int)DirectionVectors.ForeArmLeft] = DirectionVector(sk.ElbowLeft, sk.WristLeft);
result[(int)DirectionVectors.ArmLeft] = DirectionVector(sk.ShoulderLeft, sk.ElbowLeft);
result[(int)DirectionVectors.ShoulderBladeLeft] = DirectionVector(sk.ShoulderCenter, sk.ShoulderLeft);
result[(int)DirectionVectors.HandRight] = DirectionVector(sk.WristRight, sk.PalmRight);
result[(int)DirectionVectors.ForeArmRight] = DirectionVector(sk.ElbowRight, sk.WristRight);
result[(int)DirectionVectors.ArmRight] = DirectionVector(sk.ShoulderRight, sk.ElbowRight);
result[(int)DirectionVectors.ShoulderBladeRight] = DirectionVector(sk.ShoulderCenter, sk.ShoulderRight);
result[(int)DirectionVectors.BetweenHands] = DirectionVector(sk.PalmRight, sk.PalmLeft);
return result;
}
public void TestScale()
{
Vector3D v1 = new Vector3D(1, 2, 3);
v1 = 2 * v1;
TestXYZ(v1, 2, 4, 6);
}
private static Vector3D DirectionVector(Vector3D src, Vector3D dest)
{
Vector3D direction = dest - src;
direction.Normalise();
return direction;
}
public void TestSubtract()
{
Vector3D v1 = new Vector3D(1, 2, 7);
Vector3D v2 = new Vector3D(2, 8, 1);
TestXYZ(v1 - v2, -1, -6, 6);
}
private void TestDirectionIs(Vector3D expected, DirVec directionVector)
{
Vector3D actual = skeletonFeatureFrame.DirectionVector(directionVector);
string message = String.Format("Expected {0} but actual was {1}", expected, actual);
Assert.AreEqual(expected.X, actual.X, Precision, message);
Assert.AreEqual(expected.Y, actual.Y, Precision, message);
Assert.AreEqual(expected.Z, actual.Z, Precision, message);
}
private static void HandCoords(Vector3D handPos, out int videoX, out int videoY, out int depthX, out int depthY)
{
// first get depth values, then video:
HandCoords(handPos, out depthX, out depthY);
KinectHandler.Get().GetVideoCoordsFromDepth(ImageResolution.Resolution640x480, depthX, depthY, out videoX, out videoY);
}