#include "munga.h"
#pragma hdrstop

#include "origin.h"
#include "motion.h"
#include "linmtrx.h"

const Origin Origin::Identity(Point3D::Identity, Quaternion::Identity);

#if defined(USE_SIGNATURE)
	int Is_Signature_Bad(const volatile Origin *)
	{
		return false;
	}
#endif

//
//###########################################################################
//###########################################################################
//
Origin::Origin(const LinearMatrix &matrix)
{
	Check(&matrix);
	linearPosition = matrix;
	angularPosition = matrix;
}

//
//###########################################################################
//###########################################################################
//
Origin& Origin::operator=(const Origin &origin)
{
	Check_Pointer(this);
	Check(&origin);

	angularPosition = origin.angularPosition;
	linearPosition = origin.linearPosition;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Origin& Origin::operator=(const LinearMatrix &matrix)
{
	Check_Pointer(this);
	Check(&matrix);

	angularPosition = matrix;
	linearPosition = matrix;
	return *this;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical Origin::operator==(const Origin &origin) const
{
	Check(this);
	return (memcmp(this, &origin, sizeof(Origin)) == 0);
}

//
//###########################################################################
//###########################################################################
//
Origin& Origin::AddScaled(const Origin& source, const Motion& delta, Scalar t)
{
	Check_Pointer(this);
	Check(&source);
	Check(&delta);
	Verify(t >= 0.0f);

	linearPosition.AddScaled(source.linearPosition, delta.linearMotion, t);
	angularPosition.AddScaled(source.angularPosition, delta.angularMotion, t);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Origin& Origin::Lerp(const Origin &start, const Origin &end, Scalar t)
{
	Check_Pointer(this);
	Check(&start);
	Check(&end);

	linearPosition.Lerp(start.linearPosition, end.linearPosition, t);
	angularPosition.Lerp(start.angularPosition, end.angularPosition, t);
	Check(this);
	return *this;
}

//
//###########################################################################
//###########################################################################
//
std::ostream& operator<<(std::ostream& stream, const Origin& p)
{
	return stream << '{' << p.linearPosition << ',' << p.angularPosition << '}';
}

//
//###########################################################################
//###########################################################################
//
Logical Origin::TestInstance() const
{
	return angularPosition.TestInstance();
}

#if defined(TEST_CLASS)
	#include "origin.tcp"
#endif