EllipsoidOptimisationStrategy extends AbstractBinaryFunctionOp<byte[][], Vector3d, QuickEllipsoid> { static double[] calculateTorque(final QuickEllipsoid ellipsoid, final Iterable<double[]> contactPoints) { final double[] pc = ellipsoid.getCentre(); final double cx = pc[0]; final double cy = pc[1]; final double cz = pc[2]; final double[] r = ellipsoid.getRadii(); final double a = r[0]; final double b = r[1]; final double c = r[2]; final double s = 2 / (a * a); final double t = 2 / (b * b); final double u = 2 / (c * c); final double[][] rot = ellipsoid.getRotation(); final double[][] inv = QuickEllipsoid.transpose(rot); double t0 = 0; double t1 = 0; double t2 = 0; for (final double[] p : contactPoints) { final double px = p[0] - cx; final double py = p[1] - cy; final double pz = p[2] - cz; final double x = inv[0][0] * px + inv[0][1] * py + inv[0][2] * pz; final double y = inv[1][0] * px + inv[1][1] * py + inv[1][2] * pz; final double z = inv[2][0] * px + inv[2][1] * py + inv[2][2] * pz; final double nx = s * x; final double ny = t * y; final double nz = u * z; final double length = new Vector3d(nx, ny, nz).length(); final double unx = nx / length; final double uny = ny / length; final double unz = nz / length; final double ex = rot[0][0] * unx + rot[0][1] * uny + rot[0][2] * unz; final double ey = rot[1][0] * unx + rot[1][1] * uny + rot[1][2] * unz; final double ez = rot[2][0] * unx + rot[2][1] * uny + rot[2][2] * unz; final double[] torqueVector = crossProduct(px, py, pz, ex, ey, ez); t0 += torqueVector[0]; t1 += torqueVector[1]; t2 += torqueVector[2]; } return new double[]{-t0, -t1, -t2}; }  @Override QuickEllipsoid calculate(byte[][] pixels, Vector3d seedPoint);  }
@Test public void testCalculateTorque() { double[] radii = {1,2.1,3.1}; double[] centre = {3,3,2}; QuickEllipsoid e = new QuickEllipsoid(radii,centre,new double[][]{{1,0,0},{0,1,0},{0,0,1}}); double [][] vectors = new double[7][3]; vectors[0][0] = 1; vectors[1][1] = 1; vectors[2][2] = 1; vectors[3][0] = -1; vectors[4][1] = -1; vectors[5][2] = -1; final byte[][] cubeImage = getCuboidImage(); final ArrayList<double[]> contactPoints = new ArrayList<>(); EllipsoidOptimisationStrategy.findContactPointsForGivenDirections(e, contactPoints, vectors, cubeImage,6,6,6); final double[] torque = EllipsoidOptimisationStrategy.calculateTorque(e, contactPoints); assertEquals(0,torque[0],1e-12); assertEquals(0,torque[1],1e-12); assertEquals(0,torque[2],1e-12); }