path: root/src/rgb_xyz.cc

/*
    Copyright (C) 2013-2015 Carl Hetherington <cth@carlh.net>

    This file is part of libdcp.

    libdcp is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    libdcp is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with libdcp.  If not, see <http://www.gnu.org/licenses/>.

    In addition, as a special exception, the copyright holders give
    permission to link the code of portions of this program with the
    OpenSSL library under certain conditions as described in each
    individual source file, and distribute linked combinations
    including the two.

    You must obey the GNU General Public License in all respects
    for all of the code used other than OpenSSL.  If you modify
    file(s) with this exception, you may extend this exception to your
    version of the file(s), but you are not obligated to do so.  If you
    do not wish to do so, delete this exception statement from your
    version.  If you delete this exception statement from all source
    files in the program, then also delete it here.
*/

#include "rgb_xyz.h"
#include "openjpeg_image.h"
#include "colour_conversion.h"
#include "transfer_function.h"
#include "dcp_assert.h"
#include "compose.hpp"
#include <cmath>
#include <immintrin.h>

using std::min;
using std::max;
using std::cout;
using boost::shared_ptr;
using boost::optional;
using namespace dcp;

#define DCI_COEFFICIENT (48.0 / 52.37)

/** Convert an XYZ image to RGBA.
 *  @param xyz_image Image in XYZ.
 *  @param conversion Colour conversion to use.
 *  @param argb Buffer to fill with RGBA data.  The format of the data is:
 *
 *  <pre>
 *  Byte   /- 0 -------|- 1 --------|- 2 --------|- 3 --------|- 4 --------|- 5 --------| ...
 *         |(0, 0) Blue|(0, 0)Green |(0, 0) Red  |(0, 0) Alpha|(0, 1) Blue |(0, 1) Green| ...
 *  </pre>
 *
 *  So that the first byte is the blue component of the pixel at x=0, y=0, the second
 *  is the green component, and so on.
 *
 *  Lines are packed so that the second row directly follows the first.
 */
void
dcp::xyz_to_rgba (
	boost::shared_ptr<const OpenJPEGImage> xyz_image,
	ColourConversion const & conversion,
	uint8_t* argb,
	int stride
	)
{
	int const max_colour = pow (2, 16) - 1;

	struct {
		double x, y, z;
	} s;

	struct {
		double r, g, b;
	} d;

	int* xyz_x = xyz_image->data (0);
	int* xyz_y = xyz_image->data (1);
	int* xyz_z = xyz_image->data (2);

	double const * lut_in = conversion.out()->lut (12, false);
	double const * lut_out = conversion.in()->lut (16, true);
	boost::numeric::ublas::matrix<double> const matrix = conversion.xyz_to_rgb ();

	double fast_matrix[9] = {
		matrix (0, 0), matrix (0, 1), matrix (0, 2),
		matrix (1, 0), matrix (1, 1), matrix (1, 2),
		matrix (2, 0), matrix (2, 1), matrix (2, 2)
	};

	int const height = xyz_image->size().height;
	int const width = xyz_image->size().width;

	for (int y = 0; y < height; ++y) {
		uint8_t* argb_line = argb;
		for (int x = 0; x < width; ++x) {

			DCP_ASSERT (*xyz_x >= 0 && *xyz_y >= 0 && *xyz_z >= 0 && *xyz_x < 4096 && *xyz_y < 4096 && *xyz_z < 4096);

			/* In gamma LUT */
			s.x = lut_in[*xyz_x++];
			s.y = lut_in[*xyz_y++];
			s.z = lut_in[*xyz_z++];

			/* DCI companding */
			s.x /= DCI_COEFFICIENT;
			s.y /= DCI_COEFFICIENT;
			s.z /= DCI_COEFFICIENT;

			/* XYZ to RGB */
			d.r = ((s.x * fast_matrix[0]) + (s.y * fast_matrix[1]) + (s.z * fast_matrix[2]));
			d.g = ((s.x * fast_matrix[3]) + (s.y * fast_matrix[4]) + (s.z * fast_matrix[5]));
			d.b = ((s.x * fast_matrix[6]) + (s.y * fast_matrix[7]) + (s.z * fast_matrix[8]));

			d.r = min (d.r, 1.0);
			d.r = max (d.r, 0.0);

			d.g = min (d.g, 1.0);
			d.g = max (d.g, 0.0);

			d.b = min (d.b, 1.0);
			d.b = max (d.b, 0.0);

			/* Out gamma LUT */
			*argb_line++ = lut_out[lrint(d.b * max_colour)] * 0xff;
			*argb_line++ = lut_out[lrint(d.g * max_colour)] * 0xff;
			*argb_line++ = lut_out[lrint(d.r * max_colour)] * 0xff;
			*argb_line++ = 0xff;
		}

		argb += stride;
	}
}

/** Convert an XYZ image to 48bpp RGB.
 *  @param xyz_image Frame in XYZ.
 *  @param conversion Colour conversion to use.
 *  @param rgb Buffer to fill with RGB data.  Format is packed RGB
 *  16:16:16, 48bpp, 16R, 16G, 16B, with the 2-byte value for each
 *  R/G/B component stored as little-endian; i.e. AV_PIX_FMT_RGB48LE.
 *  @param stride Stride for RGB data in bytes.
 *  @param note Optional handler for any notes that may be made during the conversion (e.g. when clamping occurs).
 */
void
dcp::xyz_to_rgb (
	shared_ptr<const OpenJPEGImage> xyz_image,
	ColourConversion const & conversion,
	uint8_t* rgb,
	int stride,
	optional<NoteHandler> note
	)
{
	struct {
		double x, y, z;
	} s;

	struct {
		double r, g, b;
	} d;

	/* These should be 12-bit values from 0-4095 */
	int* xyz_x = xyz_image->data (0);
	int* xyz_y = xyz_image->data (1);
	int* xyz_z = xyz_image->data (2);

	double const * lut_in = conversion.out()->lut (12, false);
	double const * lut_out = conversion.in()->lut (16, true);
	boost::numeric::ublas::matrix<double> const matrix = conversion.xyz_to_rgb ();

	double fast_matrix[9] = {
		matrix (0, 0), matrix (0, 1), matrix (0, 2),
		matrix (1, 0), matrix (1, 1), matrix (1, 2),
		matrix (2, 0), matrix (2, 1), matrix (2, 2)
	};

	int const height = xyz_image->size().height;
	int const width = xyz_image->size().width;

	for (int y = 0; y < height; ++y) {
		uint16_t* rgb_line = reinterpret_cast<uint16_t*> (rgb + y * stride);
		for (int x = 0; x < width; ++x) {

			int cx = *xyz_x++;
			int cy = *xyz_y++;
			int cz = *xyz_z++;

			if (cx < 0 || cx > 4095) {
				if (note) {
					note.get() (DCP_NOTE, String::compose ("XYZ value %1 out of range", cx));
				}
				cx = max (min (cx, 4095), 0);
			}

			if (cy < 0 || cy > 4095) {
				if (note) {
					note.get() (DCP_NOTE, String::compose ("XYZ value %1 out of range", cy));
				}
				cy = max (min (cy, 4095), 0);
			}

			if (cz < 0 || cz > 4095) {
				if (note) {
					note.get() (DCP_NOTE, String::compose ("XYZ value %1 out of range", cz));
				}
				cz = max (min (cz, 4095), 0);
			}

			/* In gamma LUT */
			s.x = lut_in[cx];
			s.y = lut_in[cy];
			s.z = lut_in[cz];

			/* DCI companding */
			s.x /= DCI_COEFFICIENT;
			s.y /= DCI_COEFFICIENT;
			s.z /= DCI_COEFFICIENT;

			/* XYZ to RGB */
			d.r = ((s.x * fast_matrix[0]) + (s.y * fast_matrix[1]) + (s.z * fast_matrix[2]));
			d.g = ((s.x * fast_matrix[3]) + (s.y * fast_matrix[4]) + (s.z * fast_matrix[5]));
			d.b = ((s.x * fast_matrix[6]) + (s.y * fast_matrix[7]) + (s.z * fast_matrix[8]));

			d.r = min (d.r, 1.0);
			d.r = max (d.r, 0.0);

			d.g = min (d.g, 1.0);
			d.g = max (d.g, 0.0);

			d.b = min (d.b, 1.0);
			d.b = max (d.b, 0.0);

			*rgb_line++ = lrint(lut_out[lrint(d.r * 65535)] * 65535);
			*rgb_line++ = lrint(lut_out[lrint(d.g * 65535)] * 65535);
			*rgb_line++ = lrint(lut_out[lrint(d.b * 65535)] * 65535);
		}
	}
}

/** @param conversion Colour conversion.
 *  @param matrix Filled in with the product of the RGB to XYZ matrix, the Bradford transform and the DCI companding.
 */
void
dcp::combined_rgb_to_xyz (ColourConversion const & conversion, double* matrix)
{
	boost::numeric::ublas::matrix<double> const rgb_to_xyz = conversion.rgb_to_xyz ();
	boost::numeric::ublas::matrix<double> const bradford = conversion.bradford ();

	matrix[0] = (bradford (0, 0) * rgb_to_xyz (0, 0) + bradford (0, 1) * rgb_to_xyz (1, 0) + bradford (0, 2) * rgb_to_xyz (2, 0))
		* DCI_COEFFICIENT * 65535;
	matrix[1] = (bradford (0, 0) * rgb_to_xyz (0, 1) + bradford (0, 1) * rgb_to_xyz (1, 1) + bradford (0, 2) * rgb_to_xyz (2, 1))
		* DCI_COEFFICIENT * 65535;
	matrix[2] = (bradford (0, 0) * rgb_to_xyz (0, 2) + bradford (0, 1) * rgb_to_xyz (1, 2) + bradford (0, 2) * rgb_to_xyz (2, 2))
		* DCI_COEFFICIENT * 65535;
	matrix[3] = (bradford (1, 0) * rgb_to_xyz (0, 0) + bradford (1, 1) * rgb_to_xyz (1, 0) + bradford (1, 2) * rgb_to_xyz (2, 0))
		* DCI_COEFFICIENT * 65535;
	matrix[4] = (bradford (1, 0) * rgb_to_xyz (0, 1) + bradford (1, 1) * rgb_to_xyz (1, 1) + bradford (1, 2) * rgb_to_xyz (2, 1))
		* DCI_COEFFICIENT * 65535;
	matrix[5] = (bradford (1, 0) * rgb_to_xyz (0, 2) + bradford (1, 1) * rgb_to_xyz (1, 2) + bradford (1, 2) * rgb_to_xyz (2, 2))
		* DCI_COEFFICIENT * 65535;
	matrix[6] = (bradford (2, 0) * rgb_to_xyz (0, 0) + bradford (2, 1) * rgb_to_xyz (1, 0) + bradford (2, 2) * rgb_to_xyz (2, 0))
		* DCI_COEFFICIENT * 65535;
	matrix[7] = (bradford (2, 0) * rgb_to_xyz (0, 1) + bradford (2, 1) * rgb_to_xyz (1, 1) + bradford (2, 2) * rgb_to_xyz (2, 1))
		* DCI_COEFFICIENT * 65535;
	matrix[8] = (bradford (2, 0) * rgb_to_xyz (0, 2) + bradford (2, 1) * rgb_to_xyz (1, 2) + bradford (2, 2) * rgb_to_xyz (2, 2))
		* DCI_COEFFICIENT * 65535;
}

/** @param rgb RGB data; packed RGB 16:16:16, 48bpp, 16R, 16G, 16B,
 *  with the 2-byte value for each R/G/B component stored as
 *  little-endian; i.e. AV_PIX_FMT_RGB48LE.
 *  @param size size of RGB image in pixels.
 *  @param size stride of RGB data in pixels.
 */
shared_ptr<dcp::OpenJPEGImage>
dcp::rgb_to_xyz (
	uint8_t const * rgb,
	dcp::Size size,
	int stride,
	ColourConversion const & conversion
	)
{
	shared_ptr<OpenJPEGImage> xyz (new OpenJPEGImage (size));

	struct {
		double r, g, b;
	} s;

	struct {
		double x, y, z;
	} d;

	double const * lut_in = conversion.in()->lut (12, false);
	int const * lut_out = conversion.out()->lut_int (16, true, 4095);

	/* This is is the product of the RGB to XYZ matrix, the Bradford transform and the DCI companding */
	double fast_matrix[9];
	combined_rgb_to_xyz (conversion, fast_matrix);

	int* xyz_x = xyz->data (0);
	int* xyz_y = xyz->data (1);
	int* xyz_z = xyz->data (2);
	for (int y = 0; y < size.height; ++y) {
		uint16_t const * p = reinterpret_cast<uint16_t const *> (rgb + y * stride);
		for (int x = 0; x < size.width; ++x) {

			/* In gamma LUT (converting 16-bit to 12-bit) */
			s.r = lut_in[*p++ >> 4];
			s.g = lut_in[*p++ >> 4];
			s.b = lut_in[*p++ >> 4];

			/* RGB to XYZ, Bradford transform and DCI companding */
			d.x = s.r * fast_matrix[0] + s.g * fast_matrix[1] + s.b * fast_matrix[2];
			d.y = s.r * fast_matrix[3] + s.g * fast_matrix[4] + s.b * fast_matrix[5];
			d.z = s.r * fast_matrix[6] + s.g * fast_matrix[7] + s.b * fast_matrix[8];

			/* Clamp */
			d.x = max (0.0, d.x);
			d.y = max (0.0, d.y);
			d.z = max (0.0, d.z);
			d.x = min (65535.0, d.x);
			d.y = min (65535.0, d.y);
			d.z = min (65535.0, d.z);

			/* Out gamma LUT */
			*xyz_x++ = lut_out[lrint(d.x)];
			*xyz_y++ = lut_out[lrint(d.y)];
			*xyz_z++ = lut_out[lrint(d.z)];
		}
	}

	return xyz;
}


/** @param rgb RGB data arranged as 64 bits per pixel: 16 red, 16 green, 16 blue, 16 dummy (ignored).
 *  Each 2-byte value is little endian; this is effectively AV_PIX_FMT_RGBA64LE with the A
 *  being ignored.  There must be no padding bytes between lines of the image (i.e.
 *  stride must equal width).  The pointer must be aligned to a 16-byte boundary.
 *
 *  @param size size of RGB image in pixels.
 */
shared_ptr<dcp::OpenJPEGImage>
dcp::rgb_to_xyz_avx2 (
	uint8_t const * rgb,
	dcp::Size size,
	ColourConversion const & conversion
	)
{
	/* There must be no padding in the RGB as there *can* be no padding in the XYZ output
	 * and we have to keep the RGB and XYZ data pointers similarly aligned.
	 */

	shared_ptr<OpenJPEGImage> xyz (new OpenJPEGImage (size));

	float const * lut_in = conversion.in()->lut_float (12, false);
	int const * lut_out = conversion.out()->lut_int (16, true, 4095);

	/* This is is the product of the RGB to XYZ matrix, the Bradford transform and the DCI companding */
	double transform[9];
	combined_rgb_to_xyz (conversion, transform);

	__m256i* xyz_x = reinterpret_cast<__m256i*>(xyz->data(0));
	DCP_ASSERT (!(reinterpret_cast<uintptr_t>(xyz_x) % 32));
	__m256i* xyz_y = reinterpret_cast<__m256i*>(xyz->data(1));
	DCP_ASSERT (!(reinterpret_cast<uintptr_t>(xyz_y) % 32));
	__m256i* xyz_z = reinterpret_cast<__m256i*>(xyz->data(2));
	DCP_ASSERT (!(reinterpret_cast<uintptr_t>(xyz_z) % 32));

	__m256 transform_x = _mm256_set_ps (
		0, transform[2], transform[1], transform[0], 0, transform[2], transform[1], transform[0]
		);

	__m256 transform_y = _mm256_set_ps (
		0, transform[5], transform[4], transform[3], 0, transform[5], transform[4], transform[3]
		);

	__m256 transform_z = _mm256_set_ps (
		0, transform[8], transform[7], transform[6], 0, transform[8], transform[7], transform[6]
		);

	int const pixels = size.width * size.height;
	int const fast_loops = pixels / 8;
	int const slow_loops = pixels - fast_loops * 8;

	__m128i const * p = reinterpret_cast<__m128i const *> (rgb);
	DCP_ASSERT (!(reinterpret_cast<uintptr_t>(p) % 16));

	for (int i = 0; i < fast_loops; ++i) {
		// 2 pixels in each register, extended to 32-bit since we can't do gather with 16-bit words
		__m256i rgb_A = _mm256_cvtepu16_epi32(_mm_load_si128(p + 0));
		__m256i rgb_B = _mm256_cvtepu16_epi32(_mm_load_si128(p + 1));
		__m256i rgb_C = _mm256_cvtepu16_epi32(_mm_load_si128(p + 2));
		__m256i rgb_D = _mm256_cvtepu16_epi32(_mm_load_si128(p + 3));
		p += 4;

		// shift right to truncate 16-bit inputs to 12-bit
		rgb_A = _mm256_srli_epi32 (rgb_A, 4);
		rgb_B = _mm256_srli_epi32 (rgb_B, 4);
		rgb_C = _mm256_srli_epi32 (rgb_C, 4);
		rgb_D = _mm256_srli_epi32 (rgb_D, 4);

		// input gamma LUT
		__m256 lut_1_A = _mm256_i32gather_ps (lut_in, rgb_A, 4);
		__m256 lut_1_B = _mm256_i32gather_ps (lut_in, rgb_B, 4);
		__m256 lut_1_C = _mm256_i32gather_ps (lut_in, rgb_C, 4);
		__m256 lut_1_D = _mm256_i32gather_ps (lut_in, rgb_D, 4);

		// multiply for X
		__m256 x_A = _mm256_mul_ps (lut_1_A, transform_x);
		__m256 x_B = _mm256_mul_ps (lut_1_B, transform_x);
		__m256 x_C = _mm256_mul_ps (lut_1_C, transform_x);
		__m256 x_D = _mm256_mul_ps (lut_1_D, transform_x);

		// accumulate

		// B7  B6  B5  B4  B3  B2  B1  B0  +  A7  A6  A5  A4  A3  A2  A1  A0
		//                                 =  B67 B45 A67 A45 B23 B01 A23 A01
		x_A = _mm256_hadd_ps (x_A, x_B);

		// D7  D6  D5  D4  D3  D2  D1  D0  +  C7  C6  C5  C4  C3  C2  C1  C0
		//                                 =  D67 D45 C67 C45 D23 D01 C23 C01
		x_C = _mm256_hadd_ps (x_C, x_D);

		// D67 D45 C67 C45 D23 D01 C23 C01 +  B67 B45 A67 A45 B23 B01 A23 A01
		//                                 =  D47 C47 B47 A47 D03 C03 B03 A03
		x_A = _mm256_hadd_ps (x_A, x_C);

		// same for Y
		__m256 y_A = _mm256_mul_ps (lut_1_A, transform_y);
		__m256 y_B = _mm256_mul_ps (lut_1_B, transform_y);
		__m256 y_C = _mm256_mul_ps (lut_1_C, transform_y);
		__m256 y_D = _mm256_mul_ps (lut_1_D, transform_y);
		y_A = _mm256_hadd_ps (y_A, y_B);
		y_C = _mm256_hadd_ps (y_C, y_D);
		y_A = _mm256_hadd_ps (y_A, y_C);

		// and for Z
		__m256 z_A = _mm256_mul_ps (lut_1_A, transform_z);
		__m256 z_B = _mm256_mul_ps (lut_1_B, transform_z);
		__m256 z_C = _mm256_mul_ps (lut_1_C, transform_z);
		__m256 z_D = _mm256_mul_ps (lut_1_D, transform_z);
		z_A = _mm256_hadd_ps (z_A, z_B);
		z_C = _mm256_hadd_ps (z_C, z_D);
		z_A = _mm256_hadd_ps (z_A, z_C);

		// clamp
		x_A = _mm256_min_ps(x_A, _mm256_set1_ps(65535.0));
		x_A = _mm256_max_ps(x_A, _mm256_set1_ps(0.0));
		y_A = _mm256_min_ps(y_A, _mm256_set1_ps(65535.0));
		y_A = _mm256_max_ps(y_A, _mm256_set1_ps(0.0));
		z_A = _mm256_min_ps(z_A, _mm256_set1_ps(65535.0));
		z_A = _mm256_max_ps(z_A, _mm256_set1_ps(0));

		// round to int
		__m256i lut_2_x = _mm256_cvtps_epi32(_mm256_floor_ps(x_A));
		__m256i lut_2_y = _mm256_cvtps_epi32(_mm256_floor_ps(y_A));
		__m256i lut_2_z = _mm256_cvtps_epi32(_mm256_floor_ps(z_A));

		// out gamma LUT
		lut_2_x = _mm256_i32gather_epi32 (lut_out, lut_2_x, 4);
		lut_2_y = _mm256_i32gather_epi32 (lut_out, lut_2_y, 4);
		lut_2_z = _mm256_i32gather_epi32 (lut_out, lut_2_z, 4);

		// shuffle
		lut_2_x = _mm256_permutevar8x32_epi32 (lut_2_x, _mm256_set_epi32(7, 3, 6, 2, 5, 1, 4, 0));
		lut_2_y = _mm256_permutevar8x32_epi32 (lut_2_y, _mm256_set_epi32(7, 3, 6, 2, 5, 1, 4, 0));
		lut_2_z = _mm256_permutevar8x32_epi32 (lut_2_z, _mm256_set_epi32(7, 3, 6, 2, 5, 1, 4, 0));

		// write to memory
		_mm256_store_si256 (xyz_x, lut_2_x);
		_mm256_store_si256 (xyz_y, lut_2_y);
		_mm256_store_si256 (xyz_z, lut_2_z);
		xyz_x++;
		xyz_y++;
		xyz_z++;
	}

        struct {
                float r, g, b;
        } s;

        struct {
                float x, y, z;
        } d;

	uint16_t const * p_slow = reinterpret_cast<uint16_t const *> (p);
	int* xyz_x_slow = reinterpret_cast<int*> (xyz_x);
	int* xyz_y_slow = reinterpret_cast<int*> (xyz_y);
	int* xyz_z_slow = reinterpret_cast<int*> (xyz_z);

	for (int i = 0; i < slow_loops; ++i) {
		/* In gamma LUT (converting 16-bit to 12-bit) */
		s.r = lut_in[*p_slow++ >> 4];
		s.g = lut_in[*p_slow++ >> 4];
		s.b = lut_in[*p_slow++ >> 4];
		p_slow++;

		/* RGB to XYZ, Bradford transform and DCI companding */
		d.x = s.r * transform[0] + s.g * transform[1] + s.b * transform[2];
		d.y = s.r * transform[3] + s.g * transform[4] + s.b * transform[5];
		d.z = s.r * transform[6] + s.g * transform[7] + s.b * transform[8];

		/* Clamp */
		d.x = max (0.0f, d.x);
		d.y = max (0.0f, d.y);
		d.z = max (0.0f, d.z);
		d.x = min (65535.0f, d.x);
		d.y = min (65535.0f, d.y);
		d.z = min (65535.0f, d.z);

		/* Out gamma LUT */
		*xyz_x_slow++ = lut_out[lrint(d.x)];
		*xyz_y_slow++ = lut_out[lrint(d.y)];
		*xyz_z_slow++ = lut_out[lrint(d.z)];
	}

	return xyz;
}