private GeometryHandler.Vector takePointingSample(int p, double r1, double r2) { GeometryHandler.Vector v = new GeometryHandler.Vector(); if (this.mode) // System is life { while (!this.geometryHandler.vectorOK(v)) { v.reset(jointsToTrack[0], jointsToTrack[1]); // (RIGHT_ELBOW, RIGHT_HAND) } return v; } else // System is in debug-mode { switch (p) { case 0: v.reset(new Point3D(802 + r1, -92 + r1, 1821 + r1), new Point3D(619 + r2, -189 + r2, 1624 + r2)); break; case 1: v.reset(new Point3D(99 + r1, -87 + r1, 1864 + r1), new Point3D(-41 + r2, -221 + r2, 1615 + r2)); break; case 2: v.reset(new Point3D(-502 + r1, -93 + r1, 1904 + r1), new Point3D(-453 + r2, -211 + r2, 1649 + r2)); break; case 3: v.reset(new Point3D(-402 + r1, -193 + r1, 1884 + r1), new Point3D(-553 + r2, -111 + r2, 1749 + r2)); break; default: break; } return v; } }
private GeometryHandler.Vector takeLookingSample(int s, double r1, double r2) { GeometryHandler.Vector v = new GeometryHandler.Vector(); if (this.mode) // System is life { while (!this.geometryHandler.vectorOK(v)) { v.reset(jointsToTrack[4], jointsToTrack[1]); // (HEAD, RIGHT_HAND) } return v; } else // System is in debug-mode { switch (s) { case 0: v.reset(new Point3D(794 + r1, 294 + r1, 2063 + r1), new Point3D(619 + r2, -189 + r2, 1624 + r2)); break; case 1: v.reset(new Point3D(-11 + r1, 308 + r1, 2108 + r1), new Point3D(-41 + r2, -221 + r2, 1615 + r2)); break; case 2: v.reset(new Point3D(-709 + r1, 290 + r1, 2067 + r1), new Point3D(-453 + r2, -211 + r2, 1649 + r2)); break; case 3: v.reset(new Point3D(-809 + r1, 230 + r1, 1967 + r1), new Point3D(-553 + r2, -111 + r2, 1749 + r2)); break; default: break; } return v; } }