mat3 operator +(const mat3 &_lhs, const mat3 &_rhs)
{
    mat3 m;
    for(int i = 0; i < 9; ++i)
        m.set( i, _lhs.get(i) + _rhs.get(i) );
    return m;
}

void Leaf::fitBox(const mat3 &R, vec3 &center, vec3 &halfSize)
{
	vec3 x = R.row(0);
	vec3 y = R.row(1);
	vec3 z = R.row(2);
	vec3 max(-1.0e10, -1.0e10, -1.0e10);
	vec3 min( 1.0e10,  1.0e10,  1.0e10);
	std::list<Triangle>::iterator it;
	for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
		boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);
		boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);
		boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);
	}
	DBGP("Max: " << max);
	DBGP("Min: " << min);
	for (int i=0; i<3; i++) {
		halfSize[i] = 0.5 * (max[i] - min[i]);
	}
	DBGP("computed halfsize: " << halfSize);
	//halfSize = 0.5 * (max - min);
	center = min + halfSize;
	center = R.inverse() * center;
	//sanity check
	for (int i=0; i<3; i++) {
		if (halfSize[i] < TOLERANCE) {
			if (halfSize[i] < 0.5 * TOLERANCE) {
				DBGA("Warning: degenerate box computed");
			}
			halfSize[i] = TOLERANCE;
		}
	}
	DBGP("returned halfsize: " << halfSize);
}

vec3 et::removeMatrixScale(mat3& mat)
{
	vec3 c0 = mat.column(0);
	vec3 c1 = mat.column(1);
	vec3 c2 = mat.column(2);

	float lengths[3] = { c0.length(), c1.length(), c2.length() };
	
	ET_ASSERT(lengths[0] > 0.0f);
	ET_ASSERT(lengths[1] > 0.0f);
	ET_ASSERT(lengths[2] > 0.0f);

	if (mat.determinant() < 0.0f)
	{
		float minValues[3] =
		{
			etMin(c0.x, etMin(c0.y, c0.z)),
			etMin(c1.x, etMin(c1.y, c1.z)),
			etMin(c2.x, etMin(c2.y, c2.z))
		};
		auto offset = std::min_element(minValues, minValues + 3) - minValues;
		lengths[offset] = -lengths[offset];
	}

	for (size_t i = 0; i < 3; ++i)
	{
		mat[0][i] /= lengths[i];
		mat[1][i] /= lengths[i];
		mat[2][i] /= lengths[i];
	}
	
	return vec3(lengths[0], lengths[1], lengths[2]);
}

/*!
  Converts this quaternion to a 3x3 rotation matrix.
*/
void
Quaternion::ToRotationMatrix(mat3 &R) const
{
    double tx  = 2.0*x;
    double ty  = 2.0*y;
    double tz  = 2.0*z;
    double twx = tx*w;
    double twy = ty*w;
    double twz = tz*w;
    double txx = tx*x;
    double txy = ty*x;
    double txz = tz*x;
    double tyy = ty*y;
    double tyz = tz*y;
    double tzz = tz*z;

    R.element(0,0) = 1.0-(tyy+tzz);
    R.element(1,0) = txy-twz;
    R.element(2,0) = txz+twy;
    R.element(0,1) = txy+twz;
    R.element(1,1) = 1.0-(txx+tzz);
    R.element(2,1) = tyz-twx;
    R.element(0,2) = txz-twy;
    R.element(1,2) = tyz+twx;
    R.element(2,2) = 1.0-(txx+tyy);
}

void
PropertyManager::updateMat3(wxPropertyGridManager *pg, std::string label, mat3 a) {

	std::string s = label + ".Row0.x";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(0,0));
	s.clear();
	s = label + ".Row0.y";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(0, 1));
	s.clear();
	s = label + ".Row0.z";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(0, 2));

	s.clear();
	s = label + ".Row1.x";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(1, 0));
	s.clear();
	s = label + ".Row1.y";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(1, 1));
	s.clear();
	s = label + ".Row1.z";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(1, 2));

	s.clear();
	s = label + ".Row2.x";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(2, 0));
	s.clear();
	s = label + ".Row2.y";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(2, 1));
	s.clear();
	s = label + ".Row2.z";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(2, 2));
}

/* order.                                                              */
vec3 operator* (const vec3& v, const mat3& m)
{
	/* Multiply column one by the vector to get the new x component */
	float x = m.getColumn(0) * v;
	/* Multiply column two by the vector to get the new y component */
	float y = m.getColumn(1) * v;
	/* Multiply column three by the vector to get the new z component */
	float z = m.getColumn(2) * v;

	/* Return a new vector with the new components */
	return vec3(x, y, z);
}

void Graphics::setMatrix(ConstantLocation location, const mat3& value) {
	FRHICommandListImmediate& commandList = GRHICommandList.GetImmediateCommandList();
	TShaderMapRef<FVertexShaderExample> VertexShader(GetGlobalShaderMap(ERHIFeatureLevel::SM5));
	mat3 value2 = value.Transpose();
	float floats[12];
	for (int y = 0; y < 3; ++y) {
		for (int x = 0; x < 3; ++x) {
			floats[y * 4 + x] = value.get(y, x);
		}
	}
	commandList.SetShaderParameter(VertexShader->GetVertexShader(), location.parameter.GetBufferIndex(), location.parameter.GetBaseIndex(), 4 * 12, floats);
}

void ComponentPhysicsGeom::setOrientation(const mat3 &m)
{
	dMatrix3 r;

	const vec3 x = m.getAxisX().getNormal();
	const vec3 y = m.getAxisY().getNormal();
	const vec3 z = m.getAxisZ().getNormal();

	r[0] = x.x; r[1] = x.y; r[2] = x.z; r[3] = 0.0f;
	r[4] = y.x; r[5] = y.y; r[6] = y.z; r[7] = 0.0f;
	r[8] = z.x; r[9] = z.y; r[10]= z.z; r[11]= 0.0f;

	dGeomSetRotation(geom, r);

	getParentBlackBoard().relayMessage(MessageOrientationHasBeenSet(getOrientation()));
}

inline mat3 inverse(mat3 const& m)
{
	scalar_t const d = 1 / m.determinant();
	if (d != 0)
	{
		scalar_t const id = 1 / d;
		return mat3(
			vec3(
				 id * (m.y.y * m.z.z - m.y.z * m.z.y),
				-id * (m.x.y * m.z.z - m.x.z * m.z.y),
				 id * (m.x.y * m.y.z - m.x.z * m.y.y)),

			 vec3(
				-id * (m.y.x * m.z.z - m.y.z * m.z.x),
				 id * (m.x.x * m.z.z - m.x.z * m.z.x),
				-id * (m.x.x * m.y.z - m.x.z * m.y.x)),

			vec3(
				 id * (m.y.x * m.z.y - m.y.y * m.z.x),
				-id * (m.x.x * m.z.y - m.x.y * m.z.x),
				 id * (m.x.x * m.y.y - m.x.y * m.y.x)));
	}
	else
	{
		return mat3::identity();
	}
}

float determinant(const mat3 &_m)
{
    float a = _m.get(0,0);
    float b = _m.get(1,0);
    float c = _m.get(2,0);
    float d = _m.get(0,1);
    float e = _m.get(1,1);
    float f = _m.get(2,1);
    float g = _m.get(0,2);
    float h = _m.get(1,2);
    float i = _m.get(2,2);

    return a * (e*i - f*h) -
            b * (d*i - f*g) +
            c * (d*h - e*g);
}

mat3 transpose(const mat3 &_m)
{
    mat3 r;
    for(int i = 0; i < 3; ++i)
        for(int j = 0; j < 3; ++j)
            r.set(j, i, _m.get(i, j));
		return r;
}

void ComponentPhysicsGeom::drawAxes() const
{
	CHECK_GL_ERROR();
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	{
		glLineWidth(2.0f);

		const mat3 orientation = getOrientation();
		mat4 transformation(getPosition(),
			                orientation.getAxisX(),
							orientation.getAxisY(),
							orientation.getAxisZ());

		glPushMatrix();
		glMultMatrixf(transformation);

		glBegin(GL_LINES);
		glColor4fv(red);
		glVertex3fv(vec3(0,0,0));
		glVertex3fv(orientation.getAxisX());

		glColor4fv(green);
		glVertex3fv(vec3(0,0,0));
		glVertex3fv(orientation.getAxisY());

		glColor4fv(blue);
		glVertex3fv(vec3(0,0,0));
		glVertex3fv(orientation.getAxisZ());
		glEnd();

		glPopMatrix();
	}
	glPopAttrib();
	CHECK_GL_ERROR();
}

// Apply Householder reflection represented by u to column vectors of M
void reflect_cols(mat3 &M, vec3 &u)
{
    for (int i=0; i < 3; ++i) 
    {
        nv_scalar s = dot(u , M.col(i));
        for (int j=0; j < 3; ++j) 
            M(j,i) -= u[j]*s;
    }
}

bool mat3::operator ==(const mat3& rhs)
{
	for(int i = 0; i < VEC_DIM; i++)
	{
		if(v[i] != rhs.getCol(i))
			return false;
	}
	return true;
}

void Graphics::setMatrix(ConstantLocation location, const mat3& value) {
	if (location.shaderType == -1) return;
	float floats[12];
	for (int y = 0; y < 3; ++y) {
		for (int x = 0; x < 3; ++x) {
			floats[y * 4 + x] = value.get(y, x);
		}
	}
	if (location.shaderType == 0) device->SetVertexShaderConstantF(location.reg.regindex, floats, 3);
	else device->SetPixelShaderConstantF(location.reg.regindex, floats, 3);
}

mat3 operator *(const mat3 &_lhs, const mat3 &_rhs)
{
    mat3 m;
    for(int i = 0; i < 3; ++i)
        for(int j = 0; j < 3; ++j)
        {
            vec3 l (
                        _lhs.get( 0, j ),
                        _lhs.get( 1, j ),
                        _lhs.get( 2, j )
                        );
            vec3 r (
                        _rhs.get( i, 0 ),
                        _rhs.get( i, 1 ),
                        _rhs.get( i, 2 )
                        );
            m.set(i, j, dot(l, r));
        }
    return m;
}

mat3 mat3::operator *(const mat3& rhs)
{
	float a[VEC_DIM*VEC_DIM] = {0.0f};
	for(int i = 0; i < VEC_DIM; i++) {
		for(int j = 0; j < VEC_DIM; j++) {
			for(int z = 0; z < VEC_DIM; z++) {
				a[i * VEC_DIM + j] += v[i].v[z] * rhs.get(z , j);
			}
		}
	}
	return mat3(a);
}

// I had to move it here because it depends on AtomicPair class which is defined after LaueSymmetry
vector<AtomicPair> LaueSymmetry::multiply_pairs_by_matrix(vector<AtomicPair> pairs,mat3<double> transformation_matrix) {
  vector<AtomicPair>::iterator pair;
  for(pair=pairs.begin(); pair!=pairs.end(); pair++)
  {
    for(int average=0; average<2; average++)
    {
      pair->r(average)=transformation_matrix*pair->r(average);
      pair->U(average)=trusted_mat_to_sym_mat(transformation_matrix*pair->U(average)*transformation_matrix.transpose());
    }
  }
  
  return pairs;
}

	bool DiagonalizeMatrix(const mat3& m, mat3& res)
	{
		vec3 v;
		if (!EigenValues(m, v))
			return false;

		res.Identity();
		res[0] = v[0];
		res[4] = v[1];
		res[8] = v[2];

		return true;
	}

void Font::putBillboardChar(vec3 start,
							vec3 *offset,
							char character,
							const color *color,
							FontSize size,
							mat3 cameraOrientation) const
{
	const float _size = fontSize.find(size)->second;

	float w = _size / 100.0f;
	float h = _size / 100.0f;

	const vec3 right = cameraOrientation.getAxisX();
	const vec3 up = cameraOrientation.getAxisY();

	vec3 nextOffset;

	if(character == '\n')
	{
		nextOffset = (*offset) + -up*(h*lineHeight);
	}
	else
	{
		/*               center the quad          place it        shift left by an offset  */
		const vec3 a = -right*(w*0.5f)        + (*offset) - right*w*getCharacter(character).left;
		const vec3 b =  right*(w*0.5f)        + (*offset) - right*w*getCharacter(character).left;
		const vec3 c =  right*(w*0.5f) + up*h + (*offset) - right*w*getCharacter(character).left;
		const vec3 d = -right*(w*0.5f) + up*h + (*offset) - right*w*getCharacter(character).left;

		drawChar(a, b, c, d, character, *color);

		float offsetRight = w * (getCharacter(character).width + spacing);

		nextOffset = (*offset) + right*offsetRight;
	}

	(*offset) = nextOffset;
}