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); }