text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          https://www.mrpt.org/                         |
     |                                                                        |
     | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
     | See: https://www.mrpt.org/Authors - All rights reserved.               |
     | Released under BSD License. See: https://www.mrpt.org/License          |
     +------------------------------------------------------------------------+ */
  
  #include "opengl-precomp.h"	 // Precompiled header
  //
  #include <mrpt/opengl/CGeneralizedEllipsoidTemplate.h>
  #include <mrpt/opengl/opengl_api.h>
  
  using namespace mrpt;
  using namespace mrpt::opengl;
  using namespace mrpt::math;
  using namespace std;
  
  template <>
  void CGeneralizedEllipsoidTemplate<2>::implUpdate_Wireframe()
  {
  	const auto& pts = m_render_pts;
  
  	auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
  	auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
  	vbd.clear();
  
  	// Line loop:
  	const auto N = pts.size();
  	for (size_t i = 0; i < N; i++)
  	{
  		const auto ip = (i + 1) % N;
  		vbd.emplace_back(pts[i][0], pts[i][1], .0f);
  		vbd.emplace_back(pts[ip][0], pts[ip][1], .0f);
  	}
  
  	// All lines, same color:
  	cbd.assign(vbd.size(), m_color);
  }
  
  template <>
  void CGeneralizedEllipsoidTemplate<3>::implUpdate_Wireframe()
  {
  	const auto& pts = m_render_pts;
  
  	auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
  	auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
  	vbd.clear();
  
  	const auto slices = m_numSegments, stacks = m_numSegments;
  
  	// Points in the ellipsoid:
  	//  * "#slices" slices, with "#stacks" points each, but for the two ends
  	//  * 1 point at each end slice
  	// #total points = stacks*(slices-2) + 2
  	ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
  	const size_t idx_1st_slice = 1;
  
  	// 1st slice: triangle fan (if it were solid)
  	// ----------------------------
  	for (size_t i = 0; i < stacks; i++)
  	{
  		const auto idx = idx_1st_slice + i;
  		vbd.emplace_back(pts[0]);
  		vbd.emplace_back(pts[idx]);
  	}
  
  	// Middle slices: triangle strip (if it were solid)
  	// ----------------------------
  	for (size_t s = 0; s < slices - 3; s++)
  	{
  		size_t idx0 = idx_1st_slice + stacks * s;
  		size_t idx1 = idx0 + stacks;
  
  		for (size_t i = 0; i < stacks; i++)
  		{
  			const size_t ii =
  				(i == (stacks - 1) ? 0 : i + 1);  // next i with wrapping
  
  			vbd.emplace_back(pts[idx0 + i]);
  			vbd.emplace_back(pts[idx1 + ii]);
  
  			vbd.emplace_back(pts[idx1 + ii]);
  			vbd.emplace_back(pts[idx1 + i]);
  
  			vbd.emplace_back(pts[idx1 + i]);
  			vbd.emplace_back(pts[idx0 + i]);
  
  			vbd.emplace_back(pts[idx0 + i]);
  			vbd.emplace_back(pts[idx0 + ii]);
  
  			vbd.emplace_back(pts[idx0 + ii]);
  			vbd.emplace_back(pts[idx1 + ii]);
  		}
  	}
  
  	// Last slice: triangle fan (if it were solid)
  	// ----------------------------
  	const size_t idxN = pts.size() - 1;
  	const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;
  
  	for (size_t i = 0; i < stacks; i++)
  	{
  		vbd.emplace_back(pts[idxN]);
  		vbd.emplace_back(pts[idx_last_slice + i]);
  	}
  
  	// All lines, same color:
  	cbd.assign(vbd.size(), m_color);
  }
  
  template <>
  void CGeneralizedEllipsoidTemplate<2>::implUpdate_Triangles()
  {
  	using P3f = mrpt::math::TPoint3Df;
  	const auto& pts = m_render_pts;
  
  	// Render precomputed points in m_render_pts:
  	auto& tris = CRenderizableShaderTriangles::m_triangles;
  	tris.clear();
  
  	const auto N = pts.size();
  	for (size_t i = 0; i < N; i++)
  	{
  		const auto ip = (i + 1) % N;
  		tris.emplace_back(
  			P3f(0, 0, 0), P3f(pts[i][0], pts[i][1], .0f),
  			P3f(pts[ip][0], pts[ip][1], .0f));
  	}
  
  	// All faces, all vertices, same color:
  	for (auto& t : tris)
  		t.setColor(m_color);
  }
  
  template <>
  void CGeneralizedEllipsoidTemplate<3>::implUpdate_Triangles()
  {
  	const auto& pts = m_render_pts;
  
  	// Render precomputed points in m_render_pts:
  	auto& tris = CRenderizableShaderTriangles::m_triangles;
  	tris.clear();
  
  	const auto slices = m_numSegments, stacks = m_numSegments;
  
  	// Points in the ellipsoid:
  	//  * "#slices" slices, with "#stacks" points each, but for the two ends
  	//  * 1 point at each end slice
  	// #total points = stacks*(slices-2) + 2
  	ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
  	const size_t idx_1st_slice = 1;
  
  	// 1st slice: triangle fan (if it were solid)
  	// ----------------------------
  	for (size_t i = 0; i < stacks; i++)
  	{
  		const auto idx = idx_1st_slice + i;
  		const auto idxp = idx_1st_slice + ((i + 1) % stacks);
  
  		tris.emplace_back(
  			// Points
  			pts[0], pts[idxp], pts[idx],
  			// Normals:
  			pts[0], pts[idxp], pts[idx]);
  	}
  
  	// Middle slices: triangle strip (if it were solid)
  	// ----------------------------
  	for (size_t s = 0; s < slices - 3; s++)
  	{
  		size_t idx0 = idx_1st_slice + stacks * s;
  		size_t idx1 = idx0 + stacks;
  
  		for (size_t i = 0; i < stacks; i++)
  		{
  			const size_t ii =
  				(i == (stacks - 1) ? 0 : i + 1);  // next i with wrapping
  
  			tris.emplace_back(
  				// Points
  				pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i],
  				// Normals:
  				pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i]);
  			tris.emplace_back(
  				// Points
  				pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii],
  				// Normals:
  				pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii]);
  		}
  	}
  
  	// Last slice: triangle fan (if it were solid)
  	// ----------------------------
  	const size_t idxN = pts.size() - 1;
  	const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;
  
  	for (size_t i = 0; i < stacks; i++)
  	{
  		const auto idx = idx_last_slice + i;
  		const auto idxp = idx_last_slice + ((i + 1) % stacks);
  
  		tris.emplace_back(
  			// Points
  			pts[idx], pts[idxp], pts[idxN],
  			// Normals
  			pts[idx], pts[idxp], pts[idxN]);
  	}
  
  	// All faces, all vertices, same color:
  	for (auto& t : tris)
  		t.setColor(m_color);
  }
  
  template <>
  void CGeneralizedEllipsoidTemplate<2>::generatePoints(
  	const CGeneralizedEllipsoidTemplate<2>::cov_matrix_t& U,
  	std::vector<CGeneralizedEllipsoidTemplate<2>::array_parameter_t>& pts) const
  {
  	pts.clear();
  	pts.reserve(m_numSegments);
  	const double Aa = 2 * M_PI / m_numSegments;
  	for (double ang = 0; ang < 2 * M_PI; ang += Aa)
  	{
  		const double ccos = cos(ang);
  		const double ssin = sin(ang);
  
  		pts.resize(pts.size() + 1);
  
  		auto& pt = pts.back();
  
  		pt[0] = d2f(m_mean[0] + ccos * U(0, 0) + ssin * U(0, 1));
  		pt[1] = d2f(m_mean[1] + ccos * U(1, 0) + ssin * U(1, 1));
  	}
  }
  
  inline void aux_add3DpointWithEigenVectors(
  	const double x, const double y, const double z,
  	std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& pts,
  	const mrpt::math::CMatrixFixed<double, 3, 3>& M,
  	const mrpt::math::CMatrixFixed<double, 3, 1>& mean)
  {
  	pts.resize(pts.size() + 1);
  	mrpt::math::CMatrixFixed<float, 3, 1>& pt = pts.back();
  	pt[0] = d2f(mean[0] + x * M(0, 0) + y * M(0, 1) + z * M(0, 2));
  	pt[1] = d2f(mean[1] + x * M(1, 0) + y * M(1, 1) + z * M(1, 2));
  	pt[2] = d2f(mean[2] + x * M(2, 0) + y * M(2, 1) + z * M(2, 2));
  }
  
  template <>
  void CGeneralizedEllipsoidTemplate<3>::generatePoints(
  	const CGeneralizedEllipsoidTemplate<3>::cov_matrix_t& U,
  	std::vector<CGeneralizedEllipsoidTemplate<3>::array_parameter_t>& pts) const
  {
  	MRPT_START
  	const auto slices = m_numSegments, stacks = m_numSegments;
  	ASSERT_GE_(slices, 3);
  	ASSERT_GE_(stacks, 3);
  	// sin/cos cache --------
  	// Slices: [0,pi]
  	std::vector<double> slice_cos(slices), slice_sin(slices);
  	for (uint32_t i = 0; i < slices; i++)
  	{
  		double angle = M_PI * i / double(slices - 1);
  		slice_sin[i] = sin(angle);
  		slice_cos[i] = cos(angle);
  	}
  	// Stacks: [0,2*pi]
  	std::vector<double> stack_sin(stacks), stack_cos(stacks);
  	for (uint32_t i = 0; i < stacks; i++)
  	{
  		double angle = 2 * M_PI * i / double(stacks);
  		stack_sin[i] = sin(angle);
  		stack_cos[i] = cos(angle);
  	}
  
  	// Points in the ellipsoid:
  	//  * "#slices" slices, with "#stacks" points each, but for the two ends
  	//  * 1 point at each end slice
  	// #total points = stacks*(slices-2) + 2
  	pts.clear();
  	pts.reserve((slices - 2) * stacks + 2);
  
  	for (uint32_t i = 0; i < slices; i++)
  	{
  		if (i == 0) aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, m_mean);
  		else if (i == (slices - 1))
  			aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, m_mean);
  		else
  		{
  			const double x = slice_cos[i];
  			const double R = slice_sin[i];
  
  			for (uint32_t j = 0; j < stacks; j++)
  			{
  				const double y = R * stack_cos[j];
  				const double z = R * stack_sin[j];
  				aux_add3DpointWithEigenVectors(x, y, z, pts, U, m_mean);
  			}
  		}
  	}
  
  	MRPT_END
  }
  