#ifndef __RAY_PACKET_H__
#define __RAY_PACKET_H__

#include "geometry.h"
#ifndef __WXMSW__ //Linux
#include <immintrin.h>
#endif

#define USE_FMA 0

namespace VRUT
{

//if you want to use AVX, you must enable it in compiler (/arch:AVX, -mavx, ...)
#ifdef __AVX__
	class RayPacket
	{

	public:

		static const unsigned char logNumComponents=3;
		static const unsigned char numComponents=(1<<logNumComponents);

		/// point in 3d stored aligned for use AVX intrinsics
		union Point
		{
			float v[8]; ///< array of coordinates
			__m256 _p; ///< coordinate in format for AVX intrinsics
		};

		VRUT_ALIGNED(32) Point originX, originY, originZ, directionX, directionY, directionZ, invDirectionX, invDirectionY, invDirectionZ, dist;

		RayPacket() {};

		void SetRay(const Ray &ray, const float dist, unsigned int index)
		{
			originX.v[index]=ray.origin.x;
			originY.v[index]=ray.origin.y;
			originZ.v[index]=ray.origin.z;
			directionX.v[index]=ray.direction.x;
			directionY.v[index]=ray.direction.y;
			directionZ.v[index]=ray.direction.z;
			this->dist.v[index]=dist;
			if (index==numComponents-1)
				ComputeInvDirections();
		}

		void FillUnusedRays(unsigned int indexFrom)
		{
			for (size_t i = indexFrom; i < numComponents; ++i)
			{
				originX.v[i] = originX.v[0];
				originY.v[i] = originY.v[0];
				originZ.v[i] = originZ.v[0];
				directionX.v[i] = directionX.v[0];
				directionY.v[i] = directionY.v[0];
				directionZ.v[i] = directionZ.v[0];
				dist.v[i] = dist.v[0];
			}
			ComputeInvDirections();
		}

		void ComputeInvDirections()
		{
			__m256 tmpUnit = _mm256_set1_ps(1.0f);
			invDirectionX._p = _mm256_div_ps(tmpUnit, directionX._p);
			invDirectionY._p = _mm256_div_ps(tmpUnit, directionY._p);
			invDirectionZ._p = _mm256_div_ps(tmpUnit, directionZ._p);
		}

		void ToRays(Ray rays[]) const
		{
			for (size_t i = 0; i < numComponents; ++i)
			{
				rays[i].origin._v[0] = originX.v[i];
				rays[i].origin._v[1] = originY.v[i];
				rays[i].origin._v[2] = originZ.v[i];
				rays[i].direction._v[0] = directionX.v[i];
				rays[i].direction._v[1] = directionY.v[i];
				rays[i].direction._v[2] = directionZ.v[i];
			}
		}

		void SetRays(const RayPacket & ray, const Matrix & matrix)
		{
#if USE_FMA
			__m256 tmp = _mm256_set1_ps(matrix._m11);
			originX._p = _mm256_mul_ps(ray.originX._p, tmp);
			directionX._p = _mm256_mul_ps(ray.directionX._p, tmp);
			tmp = _mm256_set1_ps(matrix._m21);
			originX._p = _mm256_fmadd_ps(ray.originY._p, tmp, originX._p);
			directionX._p = _mm256_fmadd_ps(ray.directionY._p, tmp, directionX._p);
			tmp = _mm256_set1_ps(matrix._m31);
			originX._p = _mm256_fmadd_ps(ray.originZ._p, tmp, originX._p);
			directionX._p = _mm256_fmadd_ps(ray.directionZ._p, tmp, directionX._p);
			tmp = _mm256_set1_ps(matrix._m41);
			originX._p = _mm256_add_ps(originX._p, tmp);

			tmp = _mm256_set1_ps(matrix._m12);
			originY._p = _mm256_mul_ps(ray.originX._p, tmp);
			directionY._p = _mm256_mul_ps(ray.directionX._p, tmp);
			tmp = _mm256_set1_ps(matrix._m22);
			originY._p = _mm256_fmadd_ps(ray.originY._p, tmp, originY._p);
			directionY._p = _mm256_fmadd_ps(ray.directionY._p, tmp, directionY._p);
			tmp = _mm256_set1_ps(matrix._m32);
			originY._p = _mm256_fmadd_ps(ray.originZ._p, tmp, originY._p);
			directionY._p = _mm256_fmadd_ps(ray.directionZ._p, tmp, directionY._p);
			tmp = _mm256_set1_ps(matrix._m42);
			originY._p = _mm256_add_ps(originY._p, tmp);

			tmp = _mm256_set1_ps(matrix._m13);
			originZ._p = _mm256_mul_ps(ray.originX._p, tmp);
			directionZ._p = _mm256_mul_ps(ray.directionX._p, tmp);
			tmp = _mm256_set1_ps(matrix._m23);
			originZ._p = _mm256_fmadd_ps(ray.originY._p, tmp, originZ._p);
			directionZ._p = _mm256_fmadd_ps(ray.directionY._p, tmp, directionZ._p);
			tmp = _mm256_set1_ps(matrix._m33);
			originZ._p = _mm256_fmadd_ps(ray.originZ._p, tmp, originZ._p);
			directionZ._p = _mm256_fmadd_ps(ray.directionZ._p, tmp, directionZ._p);
			tmp = _mm256_set1_ps(matrix._m43);
			originZ._p = _mm256_add_ps(originZ._p, tmp);

			dist = ray.dist;

			__m256 tmpUnit = _mm256_set1_ps(1.0f);
			invDirectionX._p = _mm256_div_ps(tmpUnit, directionX._p);
			invDirectionY._p = _mm256_div_ps(tmpUnit, directionY._p);
			invDirectionZ._p = _mm256_div_ps(tmpUnit, directionZ._p);
#else
			__m256 tmp = _mm256_set1_ps(matrix._m11);
			originX._p = _mm256_mul_ps(ray.originX._p, tmp);
			directionX._p = _mm256_mul_ps(ray.directionX._p, tmp);
			tmp = _mm256_set1_ps(matrix._m21);
			originX._p = _mm256_add_ps(originX._p, _mm256_mul_ps(ray.originY._p, tmp));
			directionX._p = _mm256_add_ps(directionX._p, _mm256_mul_ps(ray.directionY._p, tmp));
			tmp = _mm256_set1_ps(matrix._m31);
			originX._p = _mm256_add_ps(originX._p, _mm256_mul_ps(ray.originZ._p, tmp));
			directionX._p = _mm256_add_ps(directionX._p, _mm256_mul_ps(ray.directionZ._p, tmp));
			tmp = _mm256_set1_ps(matrix._m41);
			originX._p = _mm256_add_ps(originX._p, tmp);

			tmp = _mm256_set1_ps(matrix._m12);
			originY._p = _mm256_mul_ps(ray.originX._p, tmp);
			directionY._p = _mm256_mul_ps(ray.directionX._p, tmp);
			tmp = _mm256_set1_ps(matrix._m22);
			originY._p = _mm256_add_ps(originY._p, _mm256_mul_ps(ray.originY._p, tmp));
			directionY._p = _mm256_add_ps(directionY._p, _mm256_mul_ps(ray.directionY._p, tmp));
			tmp = _mm256_set1_ps(matrix._m32);
			originY._p = _mm256_add_ps(originY._p, _mm256_mul_ps(ray.originZ._p, tmp));
			directionY._p = _mm256_add_ps(directionY._p, _mm256_mul_ps(ray.directionZ._p, tmp));
			tmp = _mm256_set1_ps(matrix._m42);
			originY._p = _mm256_add_ps(originY._p, tmp);

			tmp = _mm256_set1_ps(matrix._m13);
			originZ._p = _mm256_mul_ps(ray.originX._p, tmp);
			directionZ._p = _mm256_mul_ps(ray.directionX._p, tmp);
			tmp = _mm256_set1_ps(matrix._m23);
			originZ._p = _mm256_add_ps(originZ._p, _mm256_mul_ps(ray.originY._p, tmp));
			directionZ._p = _mm256_add_ps(directionZ._p, _mm256_mul_ps(ray.directionY._p, tmp));
			tmp = _mm256_set1_ps(matrix._m33);
			originZ._p = _mm256_add_ps(originZ._p, _mm256_mul_ps(ray.originZ._p, tmp));
			directionZ._p = _mm256_add_ps(directionZ._p, _mm256_mul_ps(ray.directionZ._p, tmp));
			tmp = _mm256_set1_ps(matrix._m43);
			originZ._p = _mm256_add_ps(originZ._p, tmp);

			dist = ray.dist;

			__m256 tmpUnit = _mm256_set1_ps(1.0f);
			invDirectionX._p = _mm256_div_ps(tmpUnit, directionX._p);
			invDirectionY._p = _mm256_div_ps(tmpUnit, directionY._p);
			invDirectionZ._p = _mm256_div_ps(tmpUnit, directionZ._p);
#endif
		}

		void ComputeIntersectionPointsAndDist(RayIntersectionInfo info[], const unsigned char intersection, size_t count) const
		{
			VRUT_ALIGNED(16) Point tmpOriginX, tmpOriginY, tmpOriginZ;

			tmpOriginX._p=_mm256_add_ps(originX._p, _mm256_mul_ps(directionX._p, dist._p));
			tmpOriginY._p=_mm256_add_ps(originY._p, _mm256_mul_ps(directionY._p, dist._p));
			tmpOriginZ._p=_mm256_add_ps(originZ._p, _mm256_mul_ps(directionZ._p, dist._p));
			for (size_t i = 0; i < count; ++i)
			{
				if ((intersection>>i)&1)
				{
					info[i].intersection.x=tmpOriginX.v[i];
					info[i].intersection.y=tmpOriginY.v[i];
					info[i].intersection.z=tmpOriginZ.v[i];
					info[i].dist=dist.v[i];
				}
			}
		}

		unsigned char intersectAABB(const Vector3 & pMax, const Vector3 & pMin) const
		{
#if USE_FMA
			const __m256 t0a = _mm256_fmsub_ps(_mm256_set1_ps(pMin[0]), invDirectionX._p, _mm256_mul_ps(originX._p, invDirectionX._p));
			const __m256 t1a = _mm256_fmsub_ps(_mm256_set1_ps(pMax[0]), invDirectionX._p, _mm256_mul_ps(originX._p, invDirectionX._p));
			const __m256 nra = _mm256_min_ps(t0a, t1a), fra = _mm256_max_ps(t0a, t1a);
			const __m256 t0b = _mm256_fmsub_ps(_mm256_set1_ps(pMin[1]), invDirectionX._p, _mm256_mul_ps(originX._p, invDirectionX._p));
			const __m256 t1b = _mm256_fmsub_ps(_mm256_set1_ps(pMax[1]), invDirectionX._p, _mm256_mul_ps(originX._p, invDirectionX._p));
			const __m256 nrb = _mm256_min_ps(t0b, t1b), frb = _mm256_max_ps(t0b, t1b);
			const __m256 t0c = _mm256_fmsub_ps(_mm256_set1_ps(pMin[2]), invDirectionX._p, _mm256_mul_ps(originX._p, invDirectionX._p));
			const __m256 t1c = _mm256_fmsub_ps(_mm256_set1_ps(pMax[2]), invDirectionX._p, _mm256_mul_ps(originX._p, invDirectionX._p));
			const __m256 nrc = _mm256_min_ps(t0c, t1c), frc = _mm256_max_ps(t0c, t1c);
			__m256 tmin4a = _mm256_max_ps(_mm256_max_ps(_mm256_set1_ps(0.000001f), nra), _mm256_max_ps(nrb, nrc));
			int ires = _mm256_movemask_ps(_mm256_cmp_ps( tmin4a, _mm256_min_ps(_mm256_min_ps(_mm256_set1_ps(FLT_MAX), fra), _mm256_min_ps(frb, frc)), _CMP_LE_OQ));
			if (ires != 0)
				return (ires & _mm256_movemask_ps(_mm256_cmp_ps(tmin4a, dist._p, _CMP_LE_OQ)));
			return 0;
#else
			const __m256 t0a = _mm256_mul_ps(_mm256_sub_ps(_mm256_set1_ps(pMin[0] ), originX._p), invDirectionX._p);
			const __m256 t1a = _mm256_mul_ps(_mm256_sub_ps(_mm256_set1_ps(pMax[0] ), originX._p), invDirectionX._p);
			const __m256 nra = _mm256_min_ps(t0a, t1a), fra = _mm256_max_ps(t0a, t1a);
			const __m256 t0b = _mm256_mul_ps(_mm256_sub_ps(_mm256_set1_ps(pMin[1]), originY._p), invDirectionY._p);
			const __m256 t1b = _mm256_mul_ps(_mm256_sub_ps(_mm256_set1_ps(pMax[1]), originY._p), invDirectionY._p);
			const __m256 nrb = _mm256_min_ps(t0b, t1b), frb = _mm256_max_ps(t0b, t1b);
			const __m256 t0c = _mm256_mul_ps(_mm256_sub_ps(_mm256_set1_ps(pMin[2]), originZ._p), invDirectionZ._p);
			const __m256 t1c = _mm256_mul_ps(_mm256_sub_ps(_mm256_set1_ps(pMax[2]), originZ._p), invDirectionZ._p);
			const __m256 nrc = _mm256_min_ps(t0c, t1c), frc = _mm256_max_ps( t0c, t1c );
			__m256 tmin4a = _mm256_max_ps(_mm256_max_ps(_mm256_set1_ps(0.000001f), nra), _mm256_max_ps(nrb, nrc));
			int ires = _mm256_movemask_ps(_mm256_cmp_ps( tmin4a, _mm256_min_ps(_mm256_min_ps(_mm256_set1_ps(FLT_MAX), fra), _mm256_min_ps(frb, frc)), _CMP_LE_OQ));
			if (ires != 0)
				return (ires & _mm256_movemask_ps(_mm256_cmp_ps(tmin4a, dist._p, _CMP_LE_OQ)));
			return 0;
#endif
		}

		unsigned char intersectTri(const Vector3 &v1, const Vector3 &e1, const Vector3 &e2, Point *u, Point *v)
		{
#if USE_FMA
			const __m256 e2x = _mm256_set1_ps(e2._v[0]), e2y = _mm256_set1_ps(e2._v[1]), e2z = _mm256_set1_ps(e2._v[2]);
			//const Vector3 p(direction.Cross(e2));//Vector3(y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x);
			const __m256 px = _mm256_fmsub_ps(directionY._p, e2z, _mm256_mul_ps(directionZ._p, e2y));
			const __m256 py = _mm256_fmsub_ps(directionY._p, e2x, _mm256_mul_ps(directionZ._p, e2z));
			const __m256 pz = _mm256_fmsub_ps(directionY._p, e2y, _mm256_mul_ps(directionZ._p, e2x));
			const __m256 e1x = _mm256_set1_ps(e1._v[0]), e1y = _mm256_set1_ps(e1._v[1]), e1z = _mm256_set1_ps(e1._v[2]);
			//float det = e1.Dot(p);
			const __m256 det = _mm256_fmadd_ps(e1z, pz, _mm256_fmadd_ps(e1y, py, _mm256_mul_ps(e1x, px)));
			//const Vector3 w0(origin - v1);
			const __m256 w0x = _mm256_sub_ps(originX._p, _mm256_set1_ps(v1._v[0]));
			const __m256 w0y = _mm256_sub_ps(originY._p, _mm256_set1_ps(v1._v[1]));
			const __m256 w0z = _mm256_sub_ps(originZ._p, _mm256_set1_ps(v1._v[2]));
			//u = w0.Dot(p);
			//u = u / det;
			u->_p = _mm256_div_ps(_mm256_fmadd_ps(w0z, pz, _mm256_fmadd_ps(w0y, py, _mm256_mul_ps(w0x, px))), det);
			//const Vector3 q(w0.Cross(e1));
			const __m256 qx = _mm256_fmsub_ps(w0y, e1z, _mm256_mul_ps(w0z, e1y));
			const __m256 qy = _mm256_fmsub_ps(w0z, e1x, _mm256_mul_ps(w0x, e1z));
			const __m256 qz = _mm256_fmsub_ps(w0x, e1y, _mm256_mul_ps(w0y, e1x));
			//v = direction.Dot(q);
			//v = v / det;
			v->_p = _mm256_div_ps(_mm256_fmadd_ps(directionZ._p, qz, _mm256_fmadd_ps(directionY._p, qy, _mm256_mul_ps(directionX._p, qx))), det);			
			//float t = e2.Dot(q) / det;
			const __m256 t = _mm256_div_ps(_mm256_fmadd_ps(e2z, qz, _mm256_fmadd_ps(e2y, qy, _mm256_mul_ps(e2x, qx))), det);

			const __m256 nula = _mm256_setzero_ps();
			const __m256 jedna = _mm256_set1_ps(1.0f);
			const __m256 threshold = _mm256_set1_ps(1e-6f);
			const __m256 mthreshold = _mm256_set1_ps(-1e-6f);
			const __m256 mask=_mm256_and_ps(_mm256_and_ps(_mm256_and_ps(_mm256_cmp_ps(u->_p, nula, _CMP_GE_OQ), _mm256_cmp_ps(u->_p, jedna, _CMP_LE_OQ)), _mm256_and_ps(_mm256_cmp_ps(v->_p, nula, _CMP_GE_OQ), _mm256_cmp_ps(_mm256_add_ps(u->_p, v->_p), jedna, _CMP_LE_OQ))),
				_mm256_and_ps(_mm256_and_ps(_mm256_cmp_ps(t, nula, _CMP_GE_OQ), _mm256_cmp_ps(t, dist._p, _CMP_LT_OQ)), _mm256_or_ps(_mm256_cmp_ps(det, threshold, _CMP_GE_OQ), _mm256_cmp_ps(det, mthreshold, _CMP_LE_OQ))));
			dist._p=_mm256_blendv_ps(dist._p, t, mask);
			return _mm256_movemask_ps(mask);
#else
			const __m256 e2x = _mm256_set1_ps(e2._v[0]), e2y = _mm256_set1_ps(e2._v[1]), e2z = _mm256_set1_ps(e2._v[2]);
			//const Vector3 p(direction.Cross(e2));//Vector3(y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x);
			const __m256 px = _mm256_sub_ps(_mm256_mul_ps(directionY._p, e2z), _mm256_mul_ps(directionZ._p, e2y));
			const __m256 py = _mm256_sub_ps(_mm256_mul_ps(directionZ._p, e2x), _mm256_mul_ps(directionX._p, e2z));
			const __m256 pz = _mm256_sub_ps(_mm256_mul_ps(directionX._p, e2y), _mm256_mul_ps(directionY._p, e2x));
			const __m256 e1x = _mm256_set1_ps(e1._v[0]), e1y = _mm256_set1_ps(e1._v[1]), e1z = _mm256_set1_ps(e1._v[2]);
			//float det = e1.Dot(p);
			const __m256 det = _mm256_add_ps(_mm256_add_ps(_mm256_mul_ps(e1x, px), _mm256_mul_ps(e1y, py)), _mm256_mul_ps(e1z, pz));
			//const Vector3 w0(origin - v1);
			const __m256 w0x = _mm256_sub_ps(originX._p, _mm256_set1_ps(v1._v[0]));
			const __m256 w0y = _mm256_sub_ps(originY._p, _mm256_set1_ps(v1._v[1]));
			const __m256 w0z = _mm256_sub_ps(originZ._p, _mm256_set1_ps(v1._v[2]));
			//u = w0.Dot(p);
			//u = u / det;
			u->_p = _mm256_div_ps(_mm256_add_ps(_mm256_add_ps(_mm256_mul_ps(w0x, px), _mm256_mul_ps(w0y, py)), _mm256_mul_ps(w0z, pz)), det);
			//const Vector3 q(w0.Cross(e1));
			const __m256 qx = _mm256_sub_ps(_mm256_mul_ps(w0y, e1z), _mm256_mul_ps(w0z, e1y));
			const __m256 qy = _mm256_sub_ps(_mm256_mul_ps(w0z, e1x), _mm256_mul_ps(w0x, e1z));
			const __m256 qz = _mm256_sub_ps(_mm256_mul_ps(w0x, e1y), _mm256_mul_ps(w0y, e1x));
			//v = direction.Dot(q);
			//v = v / det;
			v->_p = _mm256_div_ps(_mm256_add_ps(_mm256_add_ps(_mm256_mul_ps(directionX._p, qx), _mm256_mul_ps(directionY._p, qy)), _mm256_mul_ps(directionZ._p, qz)), det);			
			//float t = e2.Dot(q) / det;
			const __m256 t = _mm256_div_ps(_mm256_add_ps(_mm256_mul_ps(e2x, qx), _mm256_add_ps(_mm256_mul_ps(e2y, qy), _mm256_mul_ps(e2z, qz))), det);

			const __m256 nula = _mm256_setzero_ps();
			const __m256 jedna =_mm256_set1_ps(1.0f);
			const __m256 threshold = _mm256_set1_ps(1e-6f);
			const __m256 mthreshold = _mm256_set1_ps(-1e-6f);
			const __m256 mask=_mm256_and_ps(_mm256_and_ps(_mm256_and_ps(_mm256_cmp_ps(u->_p, nula, _CMP_GE_OQ), _mm256_cmp_ps(u->_p, jedna, _CMP_LE_OQ)), _mm256_and_ps(_mm256_cmp_ps(v->_p, nula, _CMP_GE_OQ), _mm256_cmp_ps(_mm256_add_ps(u->_p, v->_p), jedna, _CMP_LE_OQ))),
				_mm256_and_ps(_mm256_and_ps(_mm256_cmp_ps(t, nula, _CMP_GE_OQ), _mm256_cmp_ps(t, dist._p, _CMP_LT_OQ)), _mm256_or_ps(_mm256_cmp_ps(det, threshold, _CMP_GE_OQ), _mm256_cmp_ps(det, mthreshold, _CMP_LE_OQ))));
			dist._p=_mm256_blendv_ps(dist._p, t, mask);
			return _mm256_movemask_ps(mask);
#endif
		}
	};
#else //use SSE
	class RayPacket
	{
	public:
		static const unsigned char logNumComponents=2;
		static const unsigned char numComponents=(1<<logNumComponents);

		/// point in 3d stored aligned for use SSE intrinsics
		union Point
		{
			float v[4]; ///< array of coordinates
			__m128 _p; ///< coordinate in format for SSE intrinsics
		};
		VRUT_ALIGNED(16) Point originX, originY, originZ, directionX, directionY, directionZ, invDirectionX, invDirectionY, invDirectionZ, dist;

		RayPacket() {};

		void SetRay(const Ray &ray, const float dist, unsigned int index)
		{
			originX.v[index]=ray.origin.x;
			originY.v[index]=ray.origin.y;
			originZ.v[index]=ray.origin.z;
			directionX.v[index]=ray.direction.x;
			directionY.v[index]=ray.direction.y;
			directionZ.v[index]=ray.direction.z;
			this->dist.v[index]=dist;
			if (index==numComponents-1)
				ComputeInvDirections();
		}

		void FillUnusedRays(unsigned int indexFrom)
		{
			for (size_t i = indexFrom; i < numComponents; ++i)
			{
				originX.v[i] = originX.v[0];
				originY.v[i] = originY.v[0];
				originZ.v[i] = originZ.v[0];
				directionX.v[i] = directionX.v[0];
				directionY.v[i] = directionY.v[0];
				directionZ.v[i] = directionZ.v[0];
				dist.v[i] = dist.v[0];
			}
			ComputeInvDirections();
		}

		void ComputeInvDirections()
		{
			__m128 tmpUnit = _mm_set_ps1(1.0f);
			invDirectionX._p = _mm_div_ps(tmpUnit, directionX._p);
			invDirectionY._p = _mm_div_ps(tmpUnit, directionY._p);
			invDirectionZ._p = _mm_div_ps(tmpUnit, directionZ._p);
		}

		void ToRays(Ray rays[]) const
		{
			for (size_t i = 0; i < numComponents; ++i)
			{
				rays[i].origin._v[0]=originX.v[i];
				rays[i].origin._v[1]=originY.v[i];
				rays[i].origin._v[2]=originZ.v[i];
				rays[i].direction._v[0]=directionX.v[i];
				rays[i].direction._v[1]=directionY.v[i];
				rays[i].direction._v[2]=directionZ.v[i];
			}
		}

		void SetRays(const RayPacket &ray, const Matrix &matrix)
		{
			__m128 tmp = _mm_set_ps1(matrix._m11);
			originX._p=_mm_mul_ps(ray.originX._p, tmp);
			directionX._p=_mm_mul_ps(ray.directionX._p, tmp);
			tmp = _mm_set_ps1(matrix._m21);
			originX._p=_mm_add_ps(originX._p, _mm_mul_ps(ray.originY._p, tmp));
			directionX._p=_mm_add_ps(directionX._p, _mm_mul_ps(ray.directionY._p, tmp));
			tmp = _mm_set_ps1(matrix._m31);
			originX._p=_mm_add_ps(originX._p, _mm_mul_ps(ray.originZ._p, tmp));
			directionX._p=_mm_add_ps(directionX._p, _mm_mul_ps(ray.directionZ._p, tmp));
			tmp = _mm_set_ps1(matrix._m41);
			originX._p=_mm_add_ps(originX._p, tmp);

			tmp = _mm_set_ps1(matrix._m12);
			originY._p=_mm_mul_ps(ray.originX._p, tmp);
			directionY._p=_mm_mul_ps(ray.directionX._p, tmp);
			tmp = _mm_set_ps1(matrix._m22);
			originY._p=_mm_add_ps(originY._p, _mm_mul_ps(ray.originY._p, tmp));
			directionY._p=_mm_add_ps(directionY._p, _mm_mul_ps(ray.directionY._p, tmp));
			tmp = _mm_set_ps1(matrix._m32);
			originY._p=_mm_add_ps(originY._p, _mm_mul_ps(ray.originZ._p, tmp));
			directionY._p=_mm_add_ps(directionY._p, _mm_mul_ps(ray.directionZ._p, tmp));
			tmp = _mm_set_ps1(matrix._m42);
			originY._p=_mm_add_ps(originY._p, tmp);

			tmp = _mm_set_ps1(matrix._m13);
			originZ._p=_mm_mul_ps(ray.originX._p, tmp);
			directionZ._p=_mm_mul_ps(ray.directionX._p, tmp);
			tmp = _mm_set_ps1(matrix._m23);
			originZ._p=_mm_add_ps(originZ._p, _mm_mul_ps(ray.originY._p, tmp));
			directionZ._p=_mm_add_ps(directionZ._p, _mm_mul_ps(ray.directionY._p, tmp));
			tmp = _mm_set_ps1(matrix._m33);
			originZ._p=_mm_add_ps(originZ._p, _mm_mul_ps(ray.originZ._p, tmp));
			directionZ._p=_mm_add_ps(directionZ._p, _mm_mul_ps(ray.directionZ._p, tmp));
			tmp = _mm_set_ps1(matrix._m43);
			originZ._p=_mm_add_ps(originZ._p, tmp);

			dist=ray.dist;

			__m128 tmpUnit = _mm_set_ps1(1.0f);
			invDirectionX._p=_mm_div_ps(tmpUnit, directionX._p);
			invDirectionY._p=_mm_div_ps(tmpUnit, directionY._p);
			invDirectionZ._p=_mm_div_ps(tmpUnit, directionZ._p);
		}

		void ComputeIntersectionPointsAndDist(RayIntersectionInfo info[], const unsigned char intersection, size_t count) const
		{
			VRUT_ALIGNED(16) Point tmpOriginX, tmpOriginY, tmpOriginZ;

			tmpOriginX._p=_mm_add_ps(originX._p, _mm_mul_ps(directionX._p, dist._p));
			tmpOriginY._p=_mm_add_ps(originY._p, _mm_mul_ps(directionY._p, dist._p));
			tmpOriginZ._p=_mm_add_ps(originZ._p, _mm_mul_ps(directionZ._p, dist._p));
			for (size_t i = 0; i < count; ++i)
			{
				if ((intersection>>i)&1)
				{
					info[i].intersection.x=tmpOriginX.v[i];
					info[i].intersection.y=tmpOriginY.v[i];
					info[i].intersection.z=tmpOriginZ.v[i];
					info[i].dist=dist.v[i];
				}
			}
		}

		unsigned char intersectAABB(const Vector3 &pMax, const Vector3 &pMin) const
		{
			const __m128 minx4 = _mm_set_ps1( pMin._v[0] ), maxx4 = _mm_set_ps1( pMax._v[0] );
			const __m128 miny4 = _mm_set_ps1( pMin._v[1] ), maxy4 = _mm_set_ps1( pMax._v[1] );
			const __m128 minz4 = _mm_set_ps1( pMin._v[2] ), maxz4 = _mm_set_ps1( pMax._v[2] );
			const __m128 t0a = _mm_mul_ps( _mm_sub_ps( minx4, originX._p ), invDirectionX._p );
			const __m128 t1a = _mm_mul_ps( _mm_sub_ps( maxx4, originX._p ), invDirectionX._p );
			const __m128 nra = _mm_min_ps( t0a, t1a ), fra = _mm_max_ps( t0a, t1a );
			const __m128 t0b = _mm_mul_ps( _mm_sub_ps( miny4, originY._p ), invDirectionY._p );
			const __m128 t1b = _mm_mul_ps( _mm_sub_ps( maxy4, originY._p ), invDirectionY._p );
			const __m128 nrb = _mm_min_ps( t0b, t1b ), frb = _mm_max_ps( t0b, t1b );
			const __m128 t0c = _mm_mul_ps( _mm_sub_ps( minz4, originZ._p ), invDirectionZ._p );
			const __m128 t1c = _mm_mul_ps( _mm_sub_ps( maxz4, originZ._p ), invDirectionZ._p );
			const __m128 nrc = _mm_min_ps( t0c, t1c ), frc = _mm_max_ps( t0c, t1c );
			__m128 tmin4a = _mm_max_ps(_mm_max_ps(_mm_set_ps1(0.000001f), nra), _mm_max_ps(nrb, nrc));
			int ires = _mm_movemask_ps( _mm_cmple_ps( tmin4a, _mm_min_ps(_mm_min_ps(_mm_set_ps1(FLT_MAX), fra), _mm_min_ps(frb, frc)) ) );
			if (ires!=0)
				return (ires & _mm_movemask_ps(_mm_cmple_ps(tmin4a, dist._p)));
			return ires;
		}

		unsigned char intersectTri(const Vector3 &v1, const Vector3 &e1, const Vector3 &e2, Point *u, Point *v)
		{
			const __m128 e2x = _mm_set_ps1(e2._v[0]), e2y = _mm_set_ps1(e2._v[1]), e2z = _mm_set_ps1(e2._v[2]);
			//const Vector3 p(direction.Cross(e2));//Vector3(y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x);
			const __m128 px = _mm_sub_ps(_mm_mul_ps(directionY._p, e2z), _mm_mul_ps(directionZ._p, e2y));
			const __m128 py = _mm_sub_ps(_mm_mul_ps(directionZ._p, e2x), _mm_mul_ps(directionX._p, e2z));
			const __m128 pz = _mm_sub_ps(_mm_mul_ps(directionX._p, e2y), _mm_mul_ps(directionY._p, e2x));
			const __m128 e1x = _mm_set_ps1(e1._v[0]), e1y = _mm_set_ps1(e1._v[1]), e1z = _mm_set_ps1(e1._v[2]);
			//float det = e1.Dot(p);
			const __m128 det = _mm_add_ps(_mm_add_ps(_mm_mul_ps(e1x, px), _mm_mul_ps(e1y, py)), _mm_mul_ps(e1z, pz));
			//const Vector3 w0(origin - v1);
			const __m128 w0x = _mm_sub_ps(originX._p, _mm_set_ps1(v1._v[0]));
			const __m128 w0y = _mm_sub_ps(originY._p, _mm_set_ps1(v1._v[1]));
			const __m128 w0z = _mm_sub_ps(originZ._p, _mm_set_ps1(v1._v[2]));
			//u = w0.Dot(p);
			//u = u / det;
			u->_p = _mm_div_ps(_mm_add_ps(_mm_add_ps(_mm_mul_ps(w0x, px), _mm_mul_ps(w0y, py)), _mm_mul_ps(w0z, pz)), det);
			//const Vector3 q(w0.Cross(e1));
			const __m128 qx = _mm_sub_ps(_mm_mul_ps(w0y, e1z), _mm_mul_ps(w0z, e1y));
			const __m128 qy = _mm_sub_ps(_mm_mul_ps(w0z, e1x), _mm_mul_ps(w0x, e1z));
			const __m128 qz = _mm_sub_ps(_mm_mul_ps(w0x, e1y), _mm_mul_ps(w0y, e1x));
			//v = direction.Dot(q);
			//v = v / det;
			v->_p = _mm_div_ps(_mm_add_ps(_mm_add_ps(_mm_mul_ps(directionX._p, qx), _mm_mul_ps(directionY._p, qy)), _mm_mul_ps(directionZ._p, qz)), det);
			//float t = e2.Dot(q) / det;
			const __m128 t = _mm_div_ps(_mm_add_ps(_mm_mul_ps(e2x, qx), _mm_add_ps(_mm_mul_ps(e2y, qy), _mm_mul_ps(e2z, qz))), det);

			const __m128 nula=_mm_setzero_ps();
			const __m128 jedna=_mm_set_ps1(1.0f);
			const __m128 threshold = _mm_set_ps1(1e-6f);
			const __m128 mthreshold = _mm_set_ps1(-1e-6f);
			const __m128 mask=_mm_and_ps(_mm_and_ps(_mm_and_ps(_mm_cmpge_ps(u->_p, nula), _mm_cmple_ps(u->_p, jedna)), _mm_and_ps(_mm_cmpge_ps(v->_p, nula), _mm_cmple_ps(_mm_add_ps(u->_p, v->_p), jedna))),
				_mm_and_ps(_mm_and_ps(_mm_cmpge_ps(t, nula), _mm_cmplt_ps(t, dist._p)), _mm_or_ps(_mm_cmpge_ps(det, threshold), _mm_cmple_ps(det, mthreshold))));
			dist._p=_mm_blendv_ps(dist._p, t, mask);
			return _mm_movemask_ps(mask);
		}
	};
#endif

};

#endif /* __RAY_PACKET_H__ */
