/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 *
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source distribution.
 */

#include "dgBody.h"
#include "dgCollision.h"
#include "dgCollisionCompound.h"
#include "dgCollisionCompoundBreakable.h"
#include "dgContact.h"
#include "dgWorld.h"
#include "dgWorldDynamicUpdate.h"
#include "hpl1/engine/libraries/newton/core/dg.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

#define DG_AABB_ERROR dgFloat32(1.0e-4f)
#define DG_MAX_ANGLE_STEP dgFloat32(45.0f * dgDEG2RAD)

dgBody::dgBody() {
	//  int xx = sizeof (dgBody);
	NEWTON_ASSERT((sizeof(dgBody) & 0x0f) == 0);
}

dgBody::~dgBody() {
	//  NEWTON_ASSERT (m_collisionCell.GetCount() == 0);
}

void dgBody::SetAparentMassMatrix(const dgVector &massMatrix) {
	m_aparentMass = massMatrix;
	if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI)) {
		m_aparentMass.m_w = DG_INFINITE_MASS * 2.0f;
	}

	if (m_aparentMass.m_w >= DG_INFINITE_MASS) {
		m_aparentMass.m_x = DG_INFINITE_MASS;
		m_aparentMass.m_y = DG_INFINITE_MASS;
		m_aparentMass.m_z = DG_INFINITE_MASS;
		m_aparentMass.m_w = DG_INFINITE_MASS;
	}
}

void dgBody::SetMassMatrix(dgFloat32 mass, dgFloat32 Ix, dgFloat32 Iy,
                           dgFloat32 Iz) {
	//  NEWTON_ASSERT (mass > dgFloat32 (0.0f));
	if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI)) {
		mass = DG_INFINITE_MASS * 2.0f;
	}

	if (mass >= DG_INFINITE_MASS) {
		m_mass.m_x = DG_INFINITE_MASS;
		m_mass.m_y = DG_INFINITE_MASS;
		m_mass.m_z = DG_INFINITE_MASS;
		m_mass.m_w = DG_INFINITE_MASS;
		m_invMass.m_x = dgFloat32(0.0f);
		m_invMass.m_y = dgFloat32(0.0f);
		m_invMass.m_z = dgFloat32(0.0f);
		m_invMass.m_w = dgFloat32(0.0f);

		dgBodyMasterList &masterList(*m_world);
		if (masterList.GetFirst() != m_masterNode) {
			masterList.InsertAfter(masterList.GetFirst(), m_masterNode);
		}

	} else {
		NEWTON_ASSERT(Ix > dgFloat32(0.0f));
		NEWTON_ASSERT(Iy > dgFloat32(0.0f));
		NEWTON_ASSERT(Iz > dgFloat32(0.0f));

		//      dgVector omega (m_mass.CompProduct(m_matrix.UnrotateVector(m_omega)));

		m_mass.m_x = Ix;
		m_mass.m_y = Iy;
		m_mass.m_z = Iz;
		m_mass.m_w = mass;

		m_invMass.m_x = dgFloat32(1.0f) / Ix;
		m_invMass.m_y = dgFloat32(1.0f) / Iy;
		m_invMass.m_z = dgFloat32(1.0f) / Iz;
		m_invMass.m_w = dgFloat32(1.0f) / mass;
		//      if (m_applyGyroscopic) {
		//          m_omega = m_matrix.UnrotateVector (omega.CompProduct(m_invMass));
		//      }

		dgBodyMasterList &masterList(*m_world);
		masterList.RotateToEnd(m_masterNode);
	}

#if 0 && defined(_DEBUG) // NEWTON_ASSERT is disabled so this whole calculation is useless
	dgBodyMasterList &me = *m_world;
	dgBodyMasterList::dgListNode *refNode;
	for (refNode = me.GetFirst(); refNode; refNode = refNode->GetNext()) {
		if (refNode->GetInfo().GetBody()->m_invMass.m_w != 0.0f) {
			for (; refNode; refNode = refNode->GetNext()) {
				dgBody *body = refNode->GetInfo().GetBody();
				NEWTON_ASSERT(body->m_invMass.m_w != 0.0f);
			}
			break;
		}
	}

#endif

	SetAparentMassMatrix(m_mass);
}

void dgBody::AttachCollision(dgCollision *collision) {
	NEWTON_ASSERT(collision);
	if (collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {

		if (collision->IsType(dgCollision::dgCollisionCompoundBreakable_RTTI)) {
			dgCollisionCompoundBreakable *const compound =
			    (dgCollisionCompoundBreakable *)collision;
			collision = new (m_world->GetAllocator()) dgCollisionCompoundBreakable(
			    *compound);

		} else {
			dgCollisionCompound *const compound = (dgCollisionCompound *)collision;
			collision = new (m_world->GetAllocator()) dgCollisionCompound(*compound);
		}

	} else {
		collision->AddRef();
	}

	if (m_collision) {
		m_world->ReleaseCollision(m_collision);
		m_collision = collision;
		if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI)) {
			SetMassMatrix(m_mass.m_w, m_mass.m_x, m_mass.m_y, m_mass.m_z);
		}
		SetMatrix(m_matrix);
	} else {
		m_collision = collision;
		if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI)) {
			SetMassMatrix(m_mass.m_w, m_mass.m_x, m_mass.m_y, m_mass.m_z);
		}
	}
}

void dgBody::SetMatrix(const dgMatrix &matrix) {
	SetMatrixOriginAndRotation(matrix);

	if (!m_inCallback) {
		if (m_world->m_cpu == dgSimdPresent) {
			UpdateCollisionMatrixSimd(dgFloat32(0.0f), 0);
		} else {
			UpdateCollisionMatrix(dgFloat32(0.0f), 0);
		}
	}
}

void dgBody::SetMatrixIgnoreSleep(const dgMatrix &matrix) {
	dgBroadPhaseCollision &collisionSystem = *m_world;
	if (m_collisionCell.m_cell == &collisionSystem.m_inactiveList) {
		if (!m_spawnnedFromCallback) {
			m_world->dgGetUserLock();
			collisionSystem.Remove(this);
			collisionSystem.Add(this);
			m_world->dgReleasedUserLock();
		} else {
			collisionSystem.Remove(this);
			collisionSystem.Add(this);
		}
	}

	m_sleeping = false;
	m_prevExternalForce = dgVector(0.0f, 0.0f, 0.0f, 0.0f);
	m_prevExternalTorque = dgVector(0.0f, 0.0f, 0.0f, 0.0f);
	SetMatrix(matrix);
}

void dgBody::UpdateCollisionMatrixSimd(dgFloat32 timestep, dgInt32 threadIndex) {
#ifdef DG_BUILD_SIMD_CODE

	m_collisionWorldMatrix = m_collision->m_offset.MultiplySimd(m_matrix);

	dgVector oldP0(m_minAABB);
	dgVector oldP1(m_maxAABB);
	m_collision->CalcAABBSimd(m_collisionWorldMatrix, m_minAABB, m_maxAABB);

	if (m_continueCollisionMode) {
		dgFloat32 padding = m_collision->GetBoxMaxRadius() - m_collision->GetBoxMinRadius();
		dgFloat32 maxOmega = (m_omega % m_omega) * timestep * timestep;
		padding = (maxOmega > 1.0f) ? padding : padding * dgSqrt(maxOmega);
		dgVector step(
		    m_veloc.Scale(timestep) + m_accel.Scale(m_invMass.m_w * timestep * timestep));
		step.m_x += (step.m_x > 0.0f) ? padding : -padding;
		step.m_y += (step.m_y > 0.0f) ? padding : -padding;
		step.m_z += (step.m_z > 0.0f) ? padding : -padding;

		dgVector boxSize((m_maxAABB - m_minAABB).Scale(dgFloat32(0.25f)));
		for (dgInt32 j = 0; j < 3; j++) {
			if (dgAbsf(step[j]) > boxSize[j]) {
				if (step[j] > dgFloat32(0.0f)) {
					m_maxAABB[j] += step[j];
				} else {
					m_minAABB[j] += step[j];
				}
			}
		}

		if (m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {
			dgCollisionCompound *const compoundCollision =
			    (dgCollisionCompound *)m_collision;
			//          dgInt32 count = compoundCollision->m_count;
			//          for (dgInt32 i = 0 ; i < count; i ++) {
			//              dgVector& box1Min = compoundCollision->m_aabb[i].m_p0;
			//              dgVector& box1Max = compoundCollision->m_aabb[i].m_p1;
			dgVector &box1Min = compoundCollision->m_root->m_p0;
			dgVector &box1Max = compoundCollision->m_root->m_p1;

			dgVector boxSize((box1Max - box1Min).Scale(dgFloat32(0.25f)));
			for (dgInt32 j = 0; j < 3; j++) {
				if (dgAbsf(step[j]) > boxSize[j]) {
					if (step[j] > dgFloat32(0.0f)) {
						box1Max[j] += step[j];
					} else {
						box1Min[j] += step[j];
					}
				}
			}
			//          }
		}
	}

	if (m_collisionCell.m_cell) {
		NEWTON_ASSERT(m_world);
		if (!m_sleeping) {
			if ((dgAbsf(oldP0.m_x - m_minAABB.m_x) > DG_AABB_ERROR) || (dgAbsf(oldP0.m_y - m_minAABB.m_y) > DG_AABB_ERROR) || (dgAbsf(oldP0.m_z - m_minAABB.m_z) > DG_AABB_ERROR) || (dgAbsf(oldP1.m_x - m_maxAABB.m_x) > DG_AABB_ERROR) || (dgAbsf(oldP1.m_y - m_maxAABB.m_y) > DG_AABB_ERROR) || (dgAbsf(oldP1.m_z - m_maxAABB.m_z) > DG_AABB_ERROR)) {
				m_world->UpdateBodyBroadphase(this, threadIndex);
			} else {
				m_collisionCell.m_cell->m_active = 1;
			}
		}
	}
#endif
}

void dgBody::UpdateCollisionMatrix(dgFloat32 timestep, dgInt32 threadIndex) {
	m_collisionWorldMatrix = m_collision->m_offset * m_matrix;

	dgVector oldP0(m_minAABB);
	dgVector oldP1(m_maxAABB);
	m_collision->CalcAABB(m_collisionWorldMatrix, m_minAABB, m_maxAABB);

	if (m_continueCollisionMode) {
		dgFloat32 padding = m_collision->GetBoxMaxRadius() - m_collision->GetBoxMinRadius();
		dgFloat32 maxOmega = (m_omega % m_omega) * timestep * timestep;
		padding = (maxOmega > 1.0f) ? padding : padding * dgSqrt(maxOmega);
		dgVector step(
		    m_veloc.Scale(timestep) + m_accel.Scale(m_invMass.m_w * timestep * timestep));
		step.m_x += (step.m_x > 0.0f) ? padding : -padding;
		step.m_y += (step.m_y > 0.0f) ? padding : -padding;
		step.m_z += (step.m_z > 0.0f) ? padding : -padding;

		dgVector boxSize((m_maxAABB - m_minAABB).Scale(dgFloat32(0.25f)));
		for (dgInt32 j = 0; j < 3; j++) {
			if (dgAbsf(step[j]) > boxSize[j]) {
				if (step[j] > dgFloat32(0.0f)) {
					m_maxAABB[j] += step[j];
				} else {
					m_minAABB[j] += step[j];
				}
			}
		}

		if (m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {
			//NEWTON_ASSERT (0);

			dgCollisionCompound *const compoundCollision =
			    (dgCollisionCompound *)m_collision;
			//          dgInt32 count = compoundCollision->m_count;
			//          for (dgInt32 i = 0 ; i < count; i ++) {
			//              dgVector& box1Min = compoundCollision->m_aabb[i].m_p0;
			//              dgVector& box1Max = compoundCollision->m_aabb[i].m_p1;
			dgVector &box1Min = compoundCollision->m_root->m_p0;
			dgVector &box1Max = compoundCollision->m_root->m_p1;

			dgVector boxSizeC((box1Max - box1Min).Scale(dgFloat32(0.25f)));
			for (dgInt32 j = 0; j < 3; j++) {
				if (dgAbsf(step[j]) > boxSizeC[j]) {
					if (step[j] > dgFloat32(0.0f)) {
						box1Max[j] += step[j];
					} else {
						box1Min[j] += step[j];
					}
				}
			}
			//          }
		}
	}

	if (m_collisionCell.m_cell) {
		NEWTON_ASSERT(m_world);

		if (!m_sleeping) {
			if ((dgAbsf(oldP0.m_x - m_minAABB.m_x) > DG_AABB_ERROR) || (dgAbsf(oldP0.m_y - m_minAABB.m_y) > DG_AABB_ERROR) || (dgAbsf(oldP0.m_z - m_minAABB.m_z) > DG_AABB_ERROR) || (dgAbsf(oldP1.m_x - m_maxAABB.m_x) > DG_AABB_ERROR) || (dgAbsf(oldP1.m_y - m_maxAABB.m_y) > DG_AABB_ERROR) || (dgAbsf(oldP1.m_z - m_maxAABB.m_z) > DG_AABB_ERROR)) {
				m_world->UpdateBodyBroadphase(this, threadIndex);
			} else {
				m_collisionCell.m_cell->m_active = 1;
			}
		}
	}
}

dgFloat32 dgBody::RayCast(const dgLineBox &line, OnRayCastAction filter,
                          OnRayPrecastAction preFilter, void *const userData, dgFloat32 minT) const {
	dgContactPoint contactOut;

	NEWTON_ASSERT(filter);
	if (m_world->m_cpu == dgSimdPresent) {
		if (dgOverlapTestSimd(line.m_boxL0, line.m_boxL1, m_minAABB, m_maxAABB)) {
			dgVector localP0(m_collisionWorldMatrix.UntransformVector(line.m_l0));
			dgVector localP1(m_collisionWorldMatrix.UntransformVector(line.m_l1));
			dgFloat32 t = m_collision->RayCastSimd(localP0, localP1, contactOut,
			                                       preFilter, this, userData);
			if (t < minT) {
				NEWTON_ASSERT(t >= 0.0f);
				NEWTON_ASSERT(t <= 1.0f);

				contactOut.m_normal = m_collisionWorldMatrix.RotateVectorSimd(
				                          contactOut.m_normal);
				minT = filter(reinterpret_cast<const NewtonBody *>(this), &contactOut.m_normal.m_x, dgInt32(contactOut.m_userId),
							  userData, t);
			}
		}
	} else {
		if (dgOverlapTest(line.m_boxL0, line.m_boxL1, m_minAABB, m_maxAABB)) {
			dgVector localP0(m_collisionWorldMatrix.UntransformVector(line.m_l0));
			dgVector localP1(m_collisionWorldMatrix.UntransformVector(line.m_l1));
			dgFloat32 t = m_collision->RayCast(localP0, localP1, contactOut,
			                                   preFilter, this, userData);
			if (t < minT) {
				NEWTON_ASSERT(t >= 0.0f);
				NEWTON_ASSERT(t <= 1.0f);
				contactOut.m_normal = m_collisionWorldMatrix.RotateVector(
				                          contactOut.m_normal);
				minT = filter(reinterpret_cast<const NewtonBody *>(this), &contactOut.m_normal.m_x, dgInt32(contactOut.m_userId),
							  userData, t);
			}
		}
	}
	return minT;
}

void dgBody::AddBuoyancyForce(dgFloat32 fluidDensity,
                              dgFloat32 fluidLinearViscousity, dgFloat32 fluidAngularViscousity,
                              const dgVector &gravityVector, GetBuoyancyPlane buoyancuPlane,
                              void *const context) {
	if (m_mass.m_w > dgFloat32(1.0e-2f)) {
		dgVector volumeIntegral(
		    m_collision->CalculateVolumeIntegral(m_collisionWorldMatrix,
		            buoyancuPlane, context));
		if (volumeIntegral.m_w > dgFloat32(1.0e-4f)) {

			//          dgVector buoyanceCenter (volumeIntegral - m_matrix.m_posit);
			dgVector buoyanceCenter(volumeIntegral - m_globalCentreOfMass);

			dgVector force(gravityVector.Scale(-fluidDensity * volumeIntegral.m_w));
			dgVector torque(buoyanceCenter * force);

			dgFloat32 damp = GetMax(
			                     GetMin(
			                         (m_veloc % m_veloc) * dgFloat32(100.0f) * fluidLinearViscousity,
			                         dgFloat32(dgFloat32(1.0f))),
			                     dgFloat32(dgFloat32(10.0f)));
			force -= m_veloc.Scale(damp);

			// damp = (m_omega % m_omega) * dgFloat32 (10.0f) * fluidAngularViscousity;
			damp = GetMax(
			           GetMin(
			               (m_omega % m_omega) * dgFloat32(1000.0f) * fluidAngularViscousity,
			               dgFloat32(0.25f)),
			           dgFloat32(2.0f));
			torque -= m_omega.Scale(damp);

			//          NEWTON_ASSERT (dgSqrt (force % force) < (dgSqrt (gravityVector % gravityVector) * m_mass.m_w * dgFloat32 (100.0f)));
			//          NEWTON_ASSERT (dgSqrt (torque % torque) < (dgSqrt (gravityVector % gravityVector) * m_mass.m_w * dgFloat32 (100.0f) * dgFloat32 (10.0f)));

			m_world->dgGetUserLock();
			m_accel += force;
			m_alpha += torque;
			m_world->dgReleasedUserLock();
		}
	}
}

// void dgBody::CalcInvInertiaMatrix (dgMatrix& matrix) const
void dgBody::CalcInvInertiaMatrix() {
	NEWTON_ASSERT(m_invWorldInertiaMatrix[0][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[1][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[2][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[3][3] == dgFloat32(1.0f));

	m_invWorldInertiaMatrix[0][0] = m_invMass[0] * m_matrix[0][0];
	m_invWorldInertiaMatrix[0][1] = m_invMass[1] * m_matrix[1][0];
	m_invWorldInertiaMatrix[0][2] = m_invMass[2] * m_matrix[2][0];

	m_invWorldInertiaMatrix[1][0] = m_invMass[0] * m_matrix[0][1];
	m_invWorldInertiaMatrix[1][1] = m_invMass[1] * m_matrix[1][1];
	m_invWorldInertiaMatrix[1][2] = m_invMass[2] * m_matrix[2][1];

	m_invWorldInertiaMatrix[2][0] = m_invMass[0] * m_matrix[0][2];
	m_invWorldInertiaMatrix[2][1] = m_invMass[1] * m_matrix[1][2];
	m_invWorldInertiaMatrix[2][2] = m_invMass[2] * m_matrix[2][2];
	m_invWorldInertiaMatrix = m_invWorldInertiaMatrix * m_matrix;

	m_invWorldInertiaMatrix[3][0] = dgFloat32(0.0f);
	m_invWorldInertiaMatrix[3][1] = dgFloat32(0.0f);
	m_invWorldInertiaMatrix[3][2] = dgFloat32(0.0f);

	NEWTON_ASSERT(m_invWorldInertiaMatrix[0][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[1][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[2][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[3][3] == dgFloat32(1.0f));
}

void dgBody::CalcInvInertiaMatrixSimd() {
#ifdef DG_BUILD_SIMD_CODE
	//  simd_type m0;
	//  simd_type m1;
	//  simd_type m2;
	//  simd_type r0;
	//  simd_type r1;
	//  simd_type r2;

	simd_type r0 =
	    simd_pack_lo_v((simd_type &)m_matrix[0], (simd_type &)m_matrix[1]);
	simd_type r1 =
	    simd_pack_lo_v((simd_type &)m_matrix[2], (simd_type &)m_matrix[3]);
	simd_type m0 = simd_move_lh_v(r0, r1);
	simd_type m1 = simd_move_hl_v(r1, r0);
	r0 = simd_pack_hi_v((simd_type &)m_matrix[0], (simd_type &)m_matrix[1]);
	r1 = simd_pack_hi_v((simd_type &)m_matrix[2], (simd_type &)m_matrix[3]);
	simd_type m2 = simd_move_lh_v(r0, r1);

	r0 = simd_mul_v((simd_type &)m_matrix[0], simd_set1(m_invMass[0]));
	r1 = simd_mul_v((simd_type &)m_matrix[1], simd_set1(m_invMass[1]));
	simd_type r2 = simd_mul_v((simd_type &)m_matrix[2], simd_set1(m_invMass[2]));

	(simd_type &)m_invWorldInertiaMatrix[0] =
	    simd_mul_add_v(
	        simd_mul_add_v(simd_mul_v(r0, simd_permut_v(m0, m0, PURMUT_MASK(0, 0, 0, 0))),
	                       r1, simd_permut_v(m0, m0, PURMUT_MASK(1, 1, 1, 1))),
	        r2, simd_permut_v(m0, m0, PURMUT_MASK(2, 2, 2, 2)));

	(simd_type &)m_invWorldInertiaMatrix[1] =
	    simd_mul_add_v(
	        simd_mul_add_v(simd_mul_v(r0, simd_permut_v(m1, m1, PURMUT_MASK(0, 0, 0, 0))),
	                       r1, simd_permut_v(m1, m1, PURMUT_MASK(1, 1, 1, 1))),
	        r2, simd_permut_v(m1, m1, PURMUT_MASK(2, 2, 2, 2)));

	(simd_type &)m_invWorldInertiaMatrix[2] =
	    simd_mul_add_v(
	        simd_mul_add_v(simd_mul_v(r0, simd_permut_v(m2, m2, PURMUT_MASK(0, 0, 0, 0))),
	                       r1, simd_permut_v(m2, m2, PURMUT_MASK(1, 1, 1, 1))),
	        r2, simd_permut_v(m2, m2, PURMUT_MASK(2, 2, 2, 2)));

#ifdef _DEBUG
//	dgMatrix aaa (m_invWorldInertiaMatrix);
//	CalcInvInertiaMatrix ();
//	for (int i = 0; i < 4; i ++) {
//		for (int j = 0; j < 4; j ++) {
//			NEWTON_ASSERT (dgAbsf (aaa[i][j] - m_invWorldInertiaMatrix[i][j]) <= dgAbsf (aaa[i][j] * dgFloat32 (1.0e-3f)));
//		}
//	}
#endif

	NEWTON_ASSERT(m_invWorldInertiaMatrix[0][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[1][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[2][3] == dgFloat32(0.0f));
	NEWTON_ASSERT(m_invWorldInertiaMatrix[3][3] == dgFloat32(1.0f));

#else

#endif
}

/*
 void dgBody::IntegrateVelocity (dgFloat32 timestep, bool update)
 {
 dgFloat32 omegaMag2;
 dgFloat32 omegaAngle;
 dgFloat32 invOmegaMag;

 // IntegrateNotUpdate (timestep);

 m_globalCentreOfMass += m_veloc.Scale (timestep);
 while (((m_omega % m_omega) * timestep * timestep) > (DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP)) {
 m_omega = m_omega.Scale (dgFloat32 (0.8f));
 }

 // this is correct
 omegaMag2 = m_omega % m_omega;
 if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
 invOmegaMag = dgRsqrt (omegaMag2);
 dgVector omegaAxis (m_omega.Scale (invOmegaMag));
 omegaAngle = invOmegaMag * omegaMag2 * timestep;
 dgQuaternion rotation (omegaAxis, omegaAngle);
 m_rotation = m_rotation * rotation;
 m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
 m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
 }

 m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);

 if (update) {
 if (m_matrixUpdate) {
 m_world->dgGetUserLock();
 m_matrixUpdate (*this, m_matrix);
 m_world->dgReleasedUserLock();
 }
 UpdateCollisionMatrix (timestep);
 }
 }
 */

void dgBody::UpdateMatrix(dgFloat32 timestep, dgInt32 threadIndex) {
	if (m_matrixUpdate) {
		//      m_world->dgGetUserLock_();
		m_matrixUpdate(reinterpret_cast<const NewtonBody *>(this), &m_matrix.m_front.m_x, threadIndex);
		//      m_world->dgReleasedUserLock_();
	}
	//  UpdateCollisionMatrix (timestep, threadIndex);
	if (m_world->m_cpu == dgSimdPresent) {
		UpdateCollisionMatrixSimd(timestep, threadIndex);
	} else {
		UpdateCollisionMatrix(timestep, threadIndex);
	}
}

void dgBody::IntegrateVelocity(dgFloat32 timestep) {

	//  IntegrateNotUpdate (timestep);

	m_globalCentreOfMass += m_veloc.Scale(timestep);
	while (((m_omega % m_omega) * timestep * timestep) > (DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP)) {
		m_omega = m_omega.Scale(dgFloat32(0.8f));
	}

	// this is correct
	dgFloat32 omegaMag2 = m_omega % m_omega;
	if (omegaMag2 > ((dgFloat32(0.0125f) * dgDEG2RAD) * (dgFloat32(0.0125f) * dgDEG2RAD))) {
		dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
		dgVector omegaAxis(m_omega.Scale(invOmegaMag));
		dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
		dgQuaternion rotation(omegaAxis, omegaAngle);
		m_rotation = m_rotation * rotation;
		m_rotation.Scale(dgRsqrt(m_rotation.DotProduct(m_rotation)));
		m_matrix = dgMatrix(m_rotation, m_matrix.m_posit);
	}

	m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);

#if 0 && defined(_DEBUG) // NEWTON_ASSERT is disabled so this whole calculation is useless
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			NEWTON_ASSERT(dgCheckFloat(m_matrix[i][j]));
		}
	}

	int j0 = 1;
	int j1 = 2;
	for (int i = 0; i < 3; i++) {
		dgFloat32 val;
		NEWTON_ASSERT(m_matrix[i][3] == 0.0f);
		val = m_matrix[i] % m_matrix[i];
		NEWTON_ASSERT(dgAbsf(val - 1.0f) < 1.0e-5f);
		dgVector tmp(m_matrix[j0] * m_matrix[j1]);
		val = tmp % m_matrix[i];
		NEWTON_ASSERT(dgAbsf(val - 1.0f) < 1.0e-5f);
		j0 = j1;
		j1 = i;
	}
#endif
}

void dgBody::CalculateContinueVelocity(dgFloat32 timestep, dgVector &veloc,
                                       dgVector &omega) const {
	//  timestep = m_world->m_currTimestep;
	veloc = m_veloc + m_accel.Scale(timestep * m_invMass.m_w);

	dgVector localAlpha(m_matrix.UnrotateVector(m_alpha));
	dgVector alpha(m_matrix.RotateVector(localAlpha.CompProduct(m_invMass)));
	omega = m_omega + alpha.Scale(timestep);
}

void dgBody::CalculateContinueVelocitySimd(dgFloat32 timestep, dgVector &veloc,
        dgVector &omega) const {
#ifdef DG_BUILD_SIMD_CODE
	//  timestep = m_world->m_currTimestep;
	veloc = m_veloc + m_accel.Scale(timestep * m_invMass.m_w);

	dgVector localAlpha(m_matrix.UnrotateVectorSimd(m_alpha));
	dgVector alpha(
	    m_matrix.RotateVectorSimd(localAlpha.CompProductSimd(m_invMass)));
	omega = m_omega + alpha.Scale(timestep);
#endif
}

dgVector dgBody::GetTrajectory(const dgVector &velocParam,
                               const dgVector &omegaParam) const {
	NEWTON_ASSERT(0);
	return dgVector(0, 0, 0, 0);
	/*
	 dgFloat32 timestep;
	 dgFloat32 omegaMag2;
	 dgFloat32 omegaAngle;
	 dgFloat32 invOmegaMag;
	 dgVector rotdisp (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));

	 timestep = m_world->m_currTimestep;
	 dgVector omega (m_matrix.UnrotateVector (omegaParam));
	 omegaMag2 = omega % omega;
	 if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
	 invOmegaMag = dgRsqrt (omegaMag2);
	 dgVector omegaAxis (omega.Scale (invOmegaMag));
	 omegaAngle = invOmegaMag * omegaMag2 * timestep;
	 dgQuaternion rotation (omegaAxis, omegaAngle);
	 //m_rotation = rotation * m_rotation;
	 //m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
	 //m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
	 dgMatrix matrix (dgQuaternion (omegaAxis, omegaAngle), dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)));
	 rotdisp = matrix.RotateVector (m_collision->GetOffsetMatrix().m_posit);
	 }

	 return velocParam.Scale (timestep) + rotdisp;
	 */
}

/*
 void dgBody::IntegrateNotUpdate (dgFloat32 timestep)
 {
 dgFloat32 omegaMag2;
 dgFloat32 omegaAngle;
 dgFloat32 invOmegaMag;

 m_accel = m_accel.Scale (m_invMass.m_w);
 dgVector localAlpha (m_matrix.UnrotateVector (m_alpha));
 m_alpha = m_matrix.RotateVector (localAlpha.CompProduct (m_invMass));

 m_veloc += m_accel.Scale (timestep);
 m_omega += m_alpha.Scale (timestep);

 m_globalCentreOfMass += m_veloc.Scale (timestep);
 while (((m_omega % m_omega) * timestep * timestep) > (DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP)) {
 m_omega = m_omega.Scale (dgFloat32 (0.8f));
 }

 // this is correct
 omegaMag2 = m_omega % m_omega;
 if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
 invOmegaMag = dgRsqrt (omegaMag2);
 dgVector omegaAxis (m_omega.Scale (invOmegaMag));
 omegaAngle = invOmegaMag * omegaMag2 * timestep;
 dgQuaternion rotation (omegaAxis, omegaAngle);
 m_rotation = m_rotation * rotation;
 m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
 m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
 }

 m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
 }
 */

dgVector dgBody::CalculateInverseDynamicForce(const dgVector &desiredVeloc,
        dgFloat32 timestep) const {
	// dgWord* world;
	dgFloat32 massAccel;
	massAccel = m_mass.m_w / timestep;

	if (m_world->m_solverMode) {
		if (m_masterNode->GetInfo().GetCount() >= 2) {
			massAccel *= (dgFloat32(2.0f) * dgFloat32(LINEAR_SOLVER_SUB_STEPS) / dgFloat32(LINEAR_SOLVER_SUB_STEPS + 1));
		}
	}
	return (desiredVeloc - m_veloc).Scale(massAccel);
}

dgConstraint *dgBody::GetFirstJoint() const {
	dgBodyMasterListRow::dgListNode *node;

	for (node = m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) {
		dgConstraint *joint;
		joint = node->GetInfo().m_joint;
		if (joint->GetId() >= dgUnknownConstraintId) {
			return joint;
		}
	}
	return NULL;
}

dgConstraint *dgBody::GetNextJoint(const dgConstraint *joint) const {
	dgBodyMasterListRow::dgListNode *node;

	node = joint->GetLink0();
	if (joint->GetBody0() != this) {
		node = joint->GetLink1();
	}

	if (node->GetInfo().m_joint == joint) {
		for (node = node->GetNext(); node; node = node->GetNext()) {
			dgConstraint *jointT;
			jointT = node->GetInfo().m_joint;
			if (jointT->GetId() >= dgUnknownConstraintId) {
				return jointT;
			}
		}
	}

	return NULL;
}

dgConstraint *dgBody::GetFirstContact() const {
	dgBodyMasterListRow::dgListNode *node;

	for (node = m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) {
		dgConstraint *joint;
		joint = node->GetInfo().m_joint;
		if (joint->GetId() == dgContactConstraintId) {
			return joint;
		}
	}
	return NULL;
}

dgConstraint *dgBody::GetNextContact(const dgConstraint *joint) const {
	dgBodyMasterListRow::dgListNode *node;

	node = joint->GetLink0();
	if (joint->GetBody0() != this) {
		node = joint->GetLink1();
	}

	if (node->GetInfo().m_joint == joint) {
		for (node = node->GetNext(); node; node = node->GetNext()) {
			dgConstraint *jointT;
			jointT = node->GetInfo().m_joint;
			if (jointT->GetId() == dgContactConstraintId) {
				return jointT;
			}
		}
	}

	return NULL;
}

void dgBody::SetFreeze(bool state) {
	if (state) {
		Freeze();
	} else {
		Unfreeze();
	}
}

void dgBody::Freeze() {
	if (m_invMass.m_w > dgFloat32(0.0f)) {
		if (!m_freeze) {
			m_freeze = true;
			for (dgBodyMasterListRow::dgListNode *node =
			            m_masterNode->GetInfo().GetFirst();
			        node; node = node->GetNext()) {
				dgBody *const body = node->GetInfo().m_bodyNode;
				body->Freeze();
			}
		}
	}
}

void dgBody::Unfreeze() {
	if (m_invMass.m_w > dgFloat32(0.0f)) {
		// note this is in observation (to prevent bodies from not going to sleep  inside triggers
		//      m_equilibrium = false;
		if (m_freeze) {
			m_freeze = false;
			for (dgBodyMasterListRow::dgListNode *node =
			            m_masterNode->GetInfo().GetFirst();
			        node; node = node->GetNext()) {
				dgBody *const body = node->GetInfo().m_bodyNode;
				body->Unfreeze();
			}
		}
	}
}

dgMatrix dgBody::CalculateInertiaMatrix() const {
	dgMatrix tmpMatrix;

	tmpMatrix[0][0] = m_mass[0] * m_matrix[0][0];
	tmpMatrix[0][1] = m_mass[1] * m_matrix[1][0];
	tmpMatrix[0][2] = m_mass[2] * m_matrix[2][0];
	tmpMatrix[0][3] = dgFloat32(0.0f);

	tmpMatrix[1][0] = m_mass[0] * m_matrix[0][1];
	tmpMatrix[1][1] = m_mass[1] * m_matrix[1][1];
	tmpMatrix[1][2] = m_mass[2] * m_matrix[2][1];
	tmpMatrix[1][3] = dgFloat32(0.0f);

	tmpMatrix[2][0] = m_mass[0] * m_matrix[0][2];
	tmpMatrix[2][1] = m_mass[1] * m_matrix[1][2];
	tmpMatrix[2][2] = m_mass[2] * m_matrix[2][2];
	tmpMatrix[2][3] = dgFloat32(0.0f);

	tmpMatrix[3][0] = dgFloat32(0.0f);
	tmpMatrix[3][1] = dgFloat32(0.0f);
	tmpMatrix[3][2] = dgFloat32(0.0f);
	tmpMatrix[3][3] = dgFloat32(1.0f);
	return tmpMatrix * m_matrix;
}

dgMatrix dgBody::CalculateInvInertiaMatrix() const {
	dgMatrix tmpMatrix;

	tmpMatrix[0][0] = m_invMass[0] * m_matrix[0][0];
	tmpMatrix[0][1] = m_invMass[1] * m_matrix[1][0];
	tmpMatrix[0][2] = m_invMass[2] * m_matrix[2][0];
	tmpMatrix[0][3] = dgFloat32(0.0f);

	tmpMatrix[1][0] = m_invMass[0] * m_matrix[0][1];
	tmpMatrix[1][1] = m_invMass[1] * m_matrix[1][1];
	tmpMatrix[1][2] = m_invMass[2] * m_matrix[2][1];
	tmpMatrix[1][3] = dgFloat32(0.0f);

	tmpMatrix[2][0] = m_invMass[0] * m_matrix[0][2];
	tmpMatrix[2][1] = m_invMass[1] * m_matrix[1][2];
	tmpMatrix[2][2] = m_invMass[2] * m_matrix[2][2];
	tmpMatrix[2][3] = dgFloat32(0.0f);

	tmpMatrix[3][0] = dgFloat32(0.0f);
	tmpMatrix[3][1] = dgFloat32(0.0f);
	tmpMatrix[3][2] = dgFloat32(0.0f);
	tmpMatrix[3][3] = dgFloat32(1.0f);
	return tmpMatrix * m_matrix;
}

void dgBody::AddImpulse(const dgVector &pointDeltaVeloc,
                        const dgVector &pointPosit) {
	//  dgMatrix tmpMatrix;
	//  tmpMatrix[0][0] = m_invMass[0] * m_matrix[0][0];
	//  tmpMatrix[0][1] = m_invMass[1] * m_matrix[1][0];
	//  tmpMatrix[0][2] = m_invMass[2] * m_matrix[2][0];
	//  tmpMatrix[0][3] = dgFloat32 (0.0f);
	//  tmpMatrix[1][0] = m_invMass[0] * m_matrix[0][1];
	//  tmpMatrix[1][1] = m_invMass[1] * m_matrix[1][1];
	//  tmpMatrix[1][2] = m_invMass[2] * m_matrix[2][1];
	//  tmpMatrix[1][3] = dgFloat32 (0.0f);
	//  tmpMatrix[2][0] = m_invMass[0] * m_matrix[0][2];
	//  tmpMatrix[2][1] = m_invMass[1] * m_matrix[1][2];
	//  tmpMatrix[2][2] = m_invMass[2] * m_matrix[2][2];
	//  tmpMatrix[2][3] = dgFloat32 (0.0f);
	//  tmpMatrix[3][0] = dgFloat32 (0.0f);
	//  tmpMatrix[3][1] = dgFloat32 (0.0f);
	//  tmpMatrix[3][2] = dgFloat32 (0.0f);
	//  tmpMatrix[3][3] = dgFloat32 (1.0f);
	dgMatrix invInertia(CalculateInvInertiaMatrix());

	// get contact matrix
	dgMatrix tmp;
	//  dgVector globalContact (pointPosit - m_matrix.m_posit);
	dgVector globalContact(pointPosit - m_globalCentreOfMass);

	// globalContact[0] = dgFloat32 (0.0f);
	// globalContact[1] = dgFloat32 (0.0f);
	// globalContact[2] = dgFloat32 (0.0f);

	tmp[0][0] = dgFloat32(0.0f);
	tmp[0][1] = +globalContact[2];
	tmp[0][2] = -globalContact[1];
	tmp[0][3] = dgFloat32(0.0f);

	tmp[1][0] = -globalContact[2];
	tmp[1][1] = dgFloat32(0.0f);
	tmp[1][2] = +globalContact[0];
	tmp[1][3] = dgFloat32(0.0f);

	tmp[2][0] = +globalContact[1];
	tmp[2][1] = -globalContact[0];
	tmp[2][2] = dgFloat32(0.0f);
	tmp[2][3] = dgFloat32(0.0f);

	tmp[3][0] = dgFloat32(0.0f);
	tmp[3][1] = dgFloat32(0.0f);
	tmp[3][2] = dgFloat32(0.0f);
	tmp[3][3] = dgFloat32(1.0f);

	dgMatrix contactMatrix(tmp * invInertia * tmp);
	for (dgInt32 i = 0; i < 3; i++) {
		for (dgInt32 j = 0; j < 3; j++) {
			contactMatrix[i][j] *= -dgFloat32(1.0f);
		}
	}
	contactMatrix[0][0] += m_invMass.m_w;
	contactMatrix[1][1] += m_invMass.m_w;
	contactMatrix[2][2] += m_invMass.m_w;

	contactMatrix = contactMatrix.Symetric3by3Inverse();

	// change of momentum
	dgVector changeOfMomentum(contactMatrix.RotateVector(pointDeltaVeloc));

	dgVector dv(changeOfMomentum.Scale(m_invMass.m_w));
	dgVector dw(invInertia.RotateVector(globalContact * changeOfMomentum));

	m_veloc += dv;
	m_omega += dw;

	m_sleeping = false;
	m_equilibrium = false;
	Unfreeze();
}

void dgBody::ApplyImpulseArray(dgInt32 count, dgInt32 strideInBytes,
                               const dgFloat32 *const impulseArray, const dgFloat32 *const pointArray) {
	dgInt32 stride = strideInBytes / sizeof(dgFloat32);

	dgMatrix inertia(CalculateInertiaMatrix());

	dgVector impulse(m_veloc.Scale(m_mass.m_w));
	dgVector angularImpulse(inertia.RotateVector(m_omega));

	dgVector com(m_globalCentreOfMass);
	for (dgInt32 i = 0; i < count; i++) {
		dgInt32 index = i * stride;
		dgVector r(pointArray[index], pointArray[index + 1], pointArray[index + 2],
		           dgFloat32(0.0f));
		dgVector L(impulseArray[index], impulseArray[index + 1],
		           impulseArray[index + 2], dgFloat32(0.0f));
		dgVector Q((r - com) * L);

		impulse += L;
		angularImpulse += Q;
	}

	dgMatrix invInertia(CalculateInvInertiaMatrix());
	m_veloc = impulse.Scale(m_invMass.m_w);
	m_omega = invInertia.RotateVector(angularImpulse);

	m_sleeping = false;
	m_equilibrium = false;
	Unfreeze();
}

void dgBody::InvalidateCache() {
	m_sleeping = false;
	//  m_isInWorld = true;
	m_equilibrium = false;
	m_genericLRUMark = 0;
	m_sleepingCounter = 0;
	m_prevExternalForce = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                               dgFloat32(0.0f), dgFloat32(0.0f));
	m_prevExternalTorque = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));

	dgMatrix matrix(m_matrix);
	SetMatrixOriginAndRotation(matrix);
}