// Force Torque Utils .H

ndFloat32 NewtonVectorLength(ndVector vec);
ndFloat32 NewtonVectorLengthSqr(ndVector vec);

void AddForceAtPos(ndBodyKinematic* Body, ndVector Force, ndVector Point);
void AddForceAtRelPos(ndBodyKinematic* Body, ndVector Force, ndVector Point);
void AddRelForceAtRelPos(ndBodyKinematic* Body, ndVector Force, ndVector Point);

void Rel2AbsVector(ndBodyKinematic* Body, ndVector Vectorrel, ndVector& Vectorabs);
ndVector Rel2AbsVector(ndBodyKinematic* Body, ndVector Vectorrel);

void Rel2AbsPoint(ndBodyKinematic* Body, ndVector Pointrel, ndVector& Pointabs);
ndVector Rel2AbsPoint(ndBodyKinematic* Body, ndVector Pointrel);

ndVector AbsVelAtRelPos(ndBodyKinematic* Body, ndVector Point);
void AbsVelAtRelPos(ndBodyKinematic* Body, ndVector Point, ndVector& AbsVel);
ndVector AbsVelAtPos(ndBodyKinematic* Body, ndVector Point);
void AbsVelAtPos(ndBodyKinematic* Body, ndVector Point, ndVector& AbsVel);
ndVector AbsAccAtRelPos(ndBodyKinematic* Body, ndVector Point);

void NewtonAddForceAtPos(dgentity* NewtonBase, ndVector Force, ndVector Point);
void NewtonAddForceAtRelPos(dgentity* NewtonBase, ndVector Force, ndVector Point);
void NewtonAddRelForceAtRelPos(dgentity* NewtonBase, ndVector Force, ndVector Point);
void NewtonAddRelForceAtPos(dgentity* NewtonBase, ndVector Force, ndVector Point);

void NewtonRel2AbsPoint(dgentity* NewtonBase, ndVector Pointrel, ndVector& Pointabs);
ndVector NewtonRel2AbsPoint(dgentity* NewtonBase, ndVector Pointrel);

void NewtonRel2AbsVector(dgentity* NewtonBase, ndVector Vectorrel, ndVector& Vectorabs);
ndVector NewtonRel2AbsVector(dgentity* NewtonBase, ndVector Vectorrel);

ndVector NewtonAbsVelAtPos(dgentity* NewtonBase, ndVector Point);
void NewtonAbsVelAtPos(dgentity* NewtonBase, ndVector Point, ndVector& AbsVel);
ndVector NewtonAbsVelAtRelPos(dgentity* NewtonBase, ndVector Point);

ndVector NewtonAbsAccAtRelPos(dgentity* NewtonBase, ndVector Point);

// Force Torque Utils .CPP

ndFloat32 NewtonVectorLength(ndVector vec)
{
	return ndFloat32(glm::length(glm::make_vec4(&vec[0])));
}

ndFloat32 NewtonVectorLengthSqr(ndVector vec)
{
	return ndFloat32(glm::length2(glm::make_vec4(&vec[0])));
}

void AddForceAtPos(ndBodyKinematic* Body, ndVector Force, ndVector Point)
{
	ndVector com(Body->GetCentreOfMass());
	ndVector R(Point - Rel2AbsPoint(Body, com));
	ndVector Torque(R.CrossProduct(Force));
	ndVector cforce(Body->GetForce());
	ndVector ctorque(Body->GetTorque());
	Body->SetForce(cforce + Force);
	Body->SetTorque(ctorque + Torque);
}

void AddForceAtRelPos(ndBodyKinematic* Body, ndVector Force, ndVector Point)
{
	if (NewtonVectorLength(Force) > ndFloat32(0.0f)) {
		AddForceAtPos(Body, Force, Rel2AbsPoint(Body, Point));
	}
}

void AddRelForceAtRelPos(ndBodyKinematic* Body, ndVector Force, ndVector Point)
{
	if (NewtonVectorLength(Force) > ndFloat32(0.0f)) {
		AddForceAtPos(Body, Rel2AbsVector(Body, Force), Rel2AbsPoint(Body, Point));
	}
}

void Rel2AbsVector(ndBodyKinematic* Body, ndVector Vectorrel, ndVector& Vectorabs)
{
	ndMatrix M(Body->GetMatrix());
    ndVector P(Vectorrel.m_x, Vectorrel.m_y, Vectorrel.m_z, 0.0f); 
	Vectorabs = M.TransformVector(P);
}

ndVector Rel2AbsVector(ndBodyKinematic* Body, ndVector Vectorrel)
{
	ndMatrix M(Body->GetMatrix());
    ndVector P(Vectorrel.m_x, Vectorrel.m_y, Vectorrel.m_z, 0.0f); 
	return P;
}

void Rel2AbsPoint(ndBodyKinematic* Body, ndVector Pointrel, ndVector& Pointabs)
{
	ndMatrix M(Body->GetMatrix());
	ndVector P(ndVector(Pointrel.m_x, Pointrel.m_y, Pointrel.m_z, 1.0f));
	Pointabs = M.TransformVector(P);
}

ndVector Rel2AbsPoint(ndBodyKinematic* Body, ndVector Pointrel)
{
	ndMatrix M(Body->GetMatrix());
	ndVector P(ndVector(Pointrel.m_x, Pointrel.m_y, Pointrel.m_z, 1.0f));
	return M.TransformVector(P);
}

ndVector AbsVelAtRelPos(ndBodyKinematic* Body, ndVector Point)
{
	ndVector com(Body->GetCentreOfMass());
	ndVector CenterVel(Body->GetVelocity());
	ndVector omega(Body->GetOmega());
	return CenterVel + omega.CrossProduct(Rel2AbsPoint(Body, Point) - Rel2AbsPoint(Body, com));
}

void AbsVelAtRelPos(ndBodyKinematic* Body, ndVector Point, ndVector& AbsVel)
{
	ndVector com(Body->GetCentreOfMass());
	ndVector CenterVel(Body->GetVelocity());
	ndVector omega(Body->GetOmega());
    AbsVel = CenterVel + omega.CrossProduct(Rel2AbsPoint(Body, Point) - Rel2AbsPoint(Body, com));
}

ndVector AbsVelAtPos(ndBodyKinematic* Body, ndVector Point)
{
	ndVector com(Body->GetCentreOfMass());
	ndVector CenterVel(Body->GetVelocity());
	ndVector omega(Body->GetOmega());
	return CenterVel + omega.CrossProduct(Point - Rel2AbsPoint(Body, com));
}

void AbsVelAtPos(ndBodyKinematic* Body, ndVector Point, ndVector& AbsVel)
{
	ndVector com(Body->GetCentreOfMass());
	ndVector CenterVel(Body->GetVelocity());
	ndVector omega(Body->GetOmega());
	AbsVel = CenterVel + omega.CrossProduct(Point - Rel2AbsPoint(Body, com));
}

ndVector AbsAccAtRelPos(ndBodyKinematic* Body, ndVector Point)
{
	ndMatrix M0(ndGetIdentityMatrix());
	ndMatrix M(Body->GetMatrix());
	//
	ndVector invInertia(Body->GetInvInertia());
	ndFloat32 invMass(Body->GetInvMass());
	//

	M0 = M;

	M = M.OrthoInverse();

	ndVector Force;
	ndVector Torque;
	ndVector Alpha;
	Force = Body->GetForce();
	Torque = Body->GetTorque();

	ndVector P(Torque.m_x, Torque.m_y, Torque.m_z, 0.0f);
	Alpha = M.TransformVector(P);
	Alpha = Alpha * invInertia;

	P = (Alpha.m_x, Alpha.m_y, Alpha.m_z, 0.0f);
	Alpha = M0.TransformVector(P);

	Force = Force.Scale(invMass);
	ndVector omega(Body->GetOmega());
	ndVector com(Body->GetCentreOfMass());

	return Force + Alpha.CrossProduct(Rel2AbsPoint(Body, Point) - Rel2AbsPoint(Body, com)) + 
		           omega.CrossProduct(omega.CrossProduct(Rel2AbsPoint(Body, Point) - Rel2AbsPoint(Body, com)));
}

// Newton Force Torque Utils.
void NewtonAddForceAtPos(dgentity* NewtonBase, ndVector Force, ndVector Point)
{
	ndVector com(NewtonBase->m_physics->GetBody()->GetCentreOfMass());
	ndVector R(Point - Rel2AbsPoint(NewtonBase->m_physics->GetBodyKinematic(), com));
	ndVector Torque(R.CrossProduct(Force));
	ndVector cforce(NewtonBase->m_physics->GetBody()->GetForce());
	ndVector ctorque(NewtonBase->m_physics->GetBody()->GetTorque());
	//
	NewtonBase->m_physics->GetBody()->SetForce(cforce + Force);
	NewtonBase->m_physics->GetBody()->SetTorque(ctorque + Torque);
}

void NewtonAddForceAtRelPos(dgentity* NewtonBase, ndVector Force, ndVector Point)
{
	if (NewtonVectorLength(Force) > ndFloat32(0.0f)) {
		NewtonAddForceAtPos(NewtonBase, Force, NewtonRel2AbsPoint(NewtonBase, Point));
	}
}

void NewtonAddRelForceAtRelPos(dgentity* NewtonBase, ndVector Force, ndVector Point)
{
	if (NewtonVectorLength(Force) > ndFloat32(0.0f)) {
		NewtonAddForceAtPos(NewtonBase, NewtonRel2AbsVector(NewtonBase, Force), NewtonRel2AbsPoint(NewtonBase, Point));
	}
}

void NewtonAddRelForceAtPos(dgentity* NewtonBase, ndVector Force, ndVector Point)
{
	if (NewtonVectorLength(Force) > ndFloat32(0.0f)) {
		NewtonAddForceAtPos(NewtonBase, NewtonRel2AbsVector(NewtonBase, Force), Point);
	}
}

void NewtonRel2AbsPoint(dgentity* NewtonBase, ndVector Pointrel, ndVector& Pointabs)
{
    ndMatrix M(NewtonBase->m_physics->GetBody()->GetMatrix());
    ndVector P(Pointrel.m_x, Pointrel.m_y, Pointrel.m_z, 1.0f);
	Pointabs = M.TransformVector(P);
}

ndVector NewtonRel2AbsPoint(dgentity* NewtonBase, ndVector Pointrel)
{
	ndMatrix M(NewtonBase->m_physics->GetBody()->GetMatrix());
	ndVector P(Pointrel.m_x, Pointrel.m_y, Pointrel.m_z, 1.0f);
	return M.TransformVector(P);
}

void NewtonRel2AbsVector(dgentity* NewtonBase, ndVector Vectorrel, ndVector& Vectorabs)
{
	ndMatrix M(NewtonBase->m_physics->GetBody()->GetMatrix());
	ndVector P(Vectorrel.m_x, Vectorrel.m_y, Vectorrel.m_z, 0.0f);
	Vectorabs = M.TransformVector(P);
}

ndVector NewtonRel2AbsVector(dgentity*  NewtonBase, ndVector Vectorrel)
{
	ndMatrix M(NewtonBase->m_physics->GetBody()->GetMatrix());
	ndVector P(Vectorrel.m_x, Vectorrel.m_y, Vectorrel.m_z, 0.0f); 
	return M.TransformVector(P);
}

ndVector NewtonAbsVelAtRelPos(dgentity* NewtonBase, ndVector Point)
{
	ndVector com(NewtonBase->m_physics->GetBody()->GetCentreOfMass());
	ndVector CenterVel(NewtonBase->m_physics->GetBody()->GetVelocity());
	ndVector omega(NewtonBase->m_physics->GetBody()->GetOmega());
	return CenterVel + omega.CrossProduct(NewtonRel2AbsPoint(NewtonBase, Point) - NewtonRel2AbsPoint(NewtonBase, com));
}

ndVector NewtonAbsVelAtPos(dgentity* NewtonBase, ndVector Point)
{
	ndVector com(NewtonBase->m_physics->GetBody()->GetCentreOfMass());
	ndVector CenterVel(NewtonBase->m_physics->GetBody()->GetVelocity());
	ndVector omega(NewtonBase->m_physics->GetBody()->GetOmega());
	return CenterVel + omega.CrossProduct(Point - NewtonRel2AbsPoint(NewtonBase, com));
}

void NewtonAbsVelAtPos(dgentity* NewtonBase, ndVector Point, ndVector& AbsVel)
{	
	ndVector com(NewtonBase->m_physics->GetBody()->GetCentreOfMass());
	ndVector CenterVel(NewtonBase->m_physics->GetBody()->GetVelocity());
	ndVector omega(NewtonBase->m_physics->GetBody()->GetOmega());
	AbsVel = CenterVel + omega.CrossProduct(Point - NewtonRel2AbsPoint(NewtonBase, com));
}

ndVector NewtonAbsAccAtRelPos(dgentity* NewtonBase, ndVector Point)
{
	ndBodyKinematic* uBody(NewtonBase->m_physics->GetBody());
	ndMatrix M0(ndGetIdentityMatrix());
	ndMatrix M(uBody->GetMatrix());
	
	ndVector invInertia(uBody->GetInvInertia());
	ndFloat32 invMass(uBody->GetInvMass());

	M0 = M;

	M = M.OrthoInverse();

	ndVector Force;
	ndVector Torque;
	ndVector Alpha;
	Force = uBody->GetForce();
	Torque = uBody->GetTorque();

	ndVector P(Torque.m_x, Torque.m_y, Torque.m_z, 0.0f);
	Alpha = M.TransformVector(P);
	Alpha = Alpha * invInertia;

	P = (Alpha.m_x, Alpha.m_y, Alpha.m_z, 0.0f);
	Alpha = M0.TransformVector(P);

	Force = Force.Scale(invMass);
	ndVector omega(uBody->GetOmega());
	ndVector com(uBody->GetCentreOfMass());

	return Force + Alpha.CrossProduct(NewtonRel2AbsPoint(NewtonBase, Point) - NewtonRel2AbsPoint(NewtonBase, com)) + 
		           omega.CrossProduct(omega.CrossProduct(NewtonRel2AbsPoint(NewtonBase, Point) - NewtonRel2AbsPoint(NewtonBase, com)));
}