#include "ForceTorqueRecorder.hpp"

namespace yade { // Cannot have #include directive inside.

YADE_PLUGIN((ForceRecorder)(TorqueRecorder));

CREATE_LOGGER(ForceRecorder);
void ForceRecorder::action()
{
	totalForce = Vector3r::Zero();
	FOREACH(Body::id_t id, ids)
	{
		if (!(scene->bodies->exists(id))) continue;
		totalForce += scene->forces.getForce(id);
	};

	//Save data to a file
	out << scene->iter << " " << totalForce[0] << " " << totalForce[1] << " " << totalForce[2] << " " << totalForce.norm() << "\n";
	out.close();
}

CREATE_LOGGER(TorqueRecorder);
void TorqueRecorder::action()
{
	totalTorque      = 0;
	Vector3r tmpAxis = rotationAxis.normalized();

	FOREACH(Body::id_t id, ids)
	{
		if (!(scene->bodies->exists(id))) continue;
		Body* b = Body::byId(id, scene).get();

		Vector3r tmpPos       = b->state->pos;
		Vector3r radiusVector = tmpAxis.cross(tmpAxis.cross(tmpPos - zeroPoint));

		totalTorque += tmpAxis.dot(scene->forces.getTorque(id) + radiusVector.cross(scene->forces.getForce(id)));
	};

	//Save data to a file
	out << scene->iter << " " << totalTorque << "\n";
	out.close();
}

} // namespace yade
