//==============================================================================
/*! \file
 * OpenMesh Toolkit for mesh analysis    \n
 * Copyright (c) 2010 by Rostislav Hulik     \n
 *
 * Author:  Rostislav Hulik, rosta.hulik@gmail.com  \n
 * Date:    2010/3/2                          \n
 *
 * This file is part of software developed for support of Rostislav Hulik's dissertation thesis at dcgm-robotics@FIT group.
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file 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 Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * Description:
 * - Class representing a triangle in ray casting R-tree structure
 */

#include <OMToolkit\OMTriBSPTreeTriangle.h>

namespace OMToolkit
{
	// static constant almost zer (for float)
	static const TriCenter::ScalarT ZERO = TriCenter::ScalarT(0.0000000001);
	
	TriCenter::TriCenter()
	{

	}
	TriCenter::TriCenter(PointT &center, FaceHT face, PointT &aabbLow, PointT &aabbHigh, Types::ModuleMeshd *mesh)
	{
		m_center = center;
		m_face = face;
		m_aabb[0] = aabbLow;
		m_aabb[1] = aabbHigh;
		
		unsigned int i = 0;
		for (MeshT::FVIter vertex = mesh->fv_begin(face); vertex; ++vertex, ++i)
			m_points[i] = mesh->point(vertex);

		m_edge1 = m_points[1] - m_points[0];
		m_edge2 = m_points[2] - m_points[0];
	}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Tests intersection between ray and face (triangle)
// @param origin Ray origin - intersection is computed also in inverse direction
// @param vector Ray direction
// @param face Face handle
// @param distance Computed parameter of intersection
// @return True if face is hit
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool TriCenter::intersects(OMRay &ray, ScalarT &distance)
{
	/*float tval;
	float A = m_points[0][0] - m_points[1][0];
	float B = m_points[0][1] - m_points[1][1];
	float C = m_points[0][2] - m_points[1][2];

	float D = m_points[0][0] - m_points[2][0];
	float E = m_points[0][1] - m_points[2][1];
	float F = m_points[0][2] - m_points[2][2];

	float G = ray.m_direction[0];
	float H = ray.m_direction[1];
	float I = ray.m_direction[2];

	float J = m_points[0][0] - ray.m_direction[0];
	float K = m_points[0][1] - ray.m_direction[1];
	float L = m_points[0][2] - ray.m_direction[2];

	float EIHF = E*I - H*F;
	float GFDI = G*F - D*I;
	float DHEG = D*H - E*G;

	float denom = (A*EIHF + B*GFDI + C*DHEG);

	float beta = (J*EIHF + K*GFDI + L*DHEG)  / denom;

	if (beta <= 0.0f || beta >= 1.0f) return false;

	float AKJB = A*K - J*B;
	float JCAL = J*C - A*L;
	float BLKC = B*L - K*C;

	float gamma = (I*AKJB + H*JCAL + G*BLKC) / denom;
	if (gamma <= 0.0f || beta + gamma >= 1.0f) return false;

	tval = -(F*AKJB + E*JCAL + D*BLKC) / denom;
	if (tval >= std::numeric_limits<float>::min() && tval <= std::numeric_limits<float>::max())
	{
		distance = tval;
		return true;
	}
	return false;*/
	distance = 0;
	//PointT vertices[3];
	unsigned int i = 0;

	MeshT::Normal tvec, qvec;
	MeshT::Normal pvec = ray.m_direction % m_edge2;

	ScalarT det = m_edge1 | pvec;

	if (det > -ZERO && det < ZERO)
		return 0;

	det = 1.0/det;

	tvec = ray.m_origin - m_points[0];

	ScalarT u = (tvec | pvec) * det;
	if (u <= 0.0 || u >= 1.0)
		return 0;

	qvec = tvec % m_edge1;

	ScalarT v = (ray.m_direction | qvec) * det;

	if (v <= 0.0 || u+v >= 1.0)
		return 0;

	distance = (m_edge2 | qvec) * det;
	return 1;
}
}