/* +------------------------------------------------------------------------+
   |                     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/CSphere.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CSphere, CRenderizable, mrpt::opengl)

uint8_t CSphere::serializeGetVersion() const { return 2; }
void CSphere::serializeTo(mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	out << m_radius;
	out << (uint32_t)m_nDivsLongitude << (uint32_t)m_nDivsLatitude;
}
void CSphere::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		{
			readFromStreamRender(in);
			in >> m_radius;
			uint32_t i, j;
			in >> i >> j;
			m_nDivsLongitude = i;
			m_nDivsLatitude = j;
			if (version == 1)
			{
				bool keepRadiusIndependentEyeDistance;
				in >> keepRadiusIndependentEyeDistance;
			}

			regenerateBaseParams();
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	CRenderizable::notifyChange();
}

bool CSphere::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
	// We need to find the points of the sphere which collide with the laser
	// beam.
	// The sphere's equation is invariant to rotations (but not to
	// translations), and we can take advantage of this;
	// we'll simply transform the center and then compute the beam's points
	// whose distance to that transformed point
	// equals the sphere's radius.

	CPose3D transf = this->m_pose - o;
	double x = transf.x(), y = transf.y(), z = transf.z();
	double r2 = m_radius * m_radius;
	double dyz = y * y + z * z;
	if (dyz > r2) return false;
	double dx = sqrt(r2 - dyz);
	if (x - dx >= 0)
	{
		dist = x - dx;
		return true;
	}
	else if (x + dx >= 0)
	{
		dist = x + dx;
		return true;
	}
	else
		return false;
}

auto CSphere::getBoundingBox() const -> mrpt::math::TBoundingBox
{
	const double R = m_radius;
	return mrpt::math::TBoundingBox({-R, -R, -R}, {R, R, R}).compose(m_pose);
}

void CSphere::renderUpdateBuffers() const
{
	const_cast<CSphere*>(this)->regenerateBaseParams();
	BASE::renderUpdateBuffers();
}
