dgBody.h

/* 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.
 */

#if !defined(AFX_DGBODY_H__C16EDCD6_53C4_4C6F_A70A_591819F7187E__INCLUDED_)
#define AFX_DGBODY_H__C16EDCD6_53C4_4C6F_A70A_591819F7187E__INCLUDED_

#include "dgBodyMasterList.h"
#include "hpl1/engine/libraries/newton/core/dg.h"


class dgLink;
class dgBody;
class dgWorld;
class dgCollision;
class dgBroadPhaseCell;

#define DG_MIN_SPEED_ATT dgFloat32(0.0f)
#define DG_MAX_SPEED_ATT dgFloat32(0.02f)
#define DG_INFINITE_MASS dgFloat32(1.0e15f)
#define DG_FREEZE_MAG dgFloat32(0.1f)
#define DG_FREEZE_MAG2 dgFloat32(DG_FREEZE_MAG *DG_FREEZE_MAG)

#define DG_ErrTolerance (1.0e-2f)
#define DG_ErrTolerance2 (DG_ErrTolerance * DG_ErrTolerance)

DG_MSC_VECTOR_ALIGMENT
struct dgLineBox {
    dgVector m_l0;
    dgVector m_l1;
    dgVector m_boxL0;
    dgVector m_boxL1;
} DG_GCC_VECTOR_ALIGMENT;

class dgConvexCastReturnInfo {
public:
    dgFloat32 m_point[4];            // collision point in global space
    dgFloat32 m_normal[4];           // surface normal at collision point in global space
    dgFloat32 m_normalOnHitPoint[4]; // surface normal at the surface of the hit body,
    // is the same as the normal calculate by a raycast passing by the hit point in the direction of the cast
    dgFloat32 m_penetration;         // contact penetration at collision point
    dgInt32 m_contaID;               // collision ID at contact point
    const dgBody *m_hitBody;         // body hit at contact point
};

typedef void(dgApi *OnBodyDestroy)(const NewtonBody *const me);
typedef void(dgApi *OnApplyExtForceAndTorque)(NewtonBody *const me, dFloat timestep, int32 threadIndex);
typedef void(dgApi *OnMatrixUpdateCallback)(const NewtonBody *const body, const dFloat *const matrix, int32 threadIndex);
typedef dgUnsigned32(dgApi *OnRayPrecastAction)(const NewtonBody *const body, const NewtonCollision *const collision, void *const userData);
typedef dgFloat32(dgApi *OnRayCastAction)(const NewtonBody *const body, const dFloat *const hitNormal, int collisionID, void *const userData, dFloat intersectParam);
typedef dgUnsigned32(dgApi *GetBuoyancyPlane)(const int32 collisionID, void *const context, const dFloat *const globalSpaceMatrix, dFloat *const globalSpacePlane);

#define OverlapTest(body0, body1) dgOverlapTest((body0)->m_minAABB, (body0)->m_maxAABB, (body1)->m_minAABB, (body1)->m_maxAABB)
//#define OverlapTest_SSE(body0,body1) dgOverlapTest_SSE ((body0)->m_minAABB, (body0)->m_maxAABB, (body1)->m_minAABB, (body1)->m_maxAABB)

class dgBroadPhaseList {
public:
    dgBroadPhaseCell *m_cell;
    void *m_axisArrayNode[3];

    void reset() {
        m_cell = NULL;
        for (uint i = 0; i < ARRAYSIZE(m_axisArrayNode); i++) m_axisArrayNode[i] = NULL;
    }
};

DG_MSC_VECTOR_ALIGMENT
class dgBody {
public:
    DG_CLASS_ALLOCATOR(allocator)

    dgBody();
    ~dgBody();

    void AddForce(const dgVector &force);
    void AddTorque(const dgVector &torque);
    void AttachCollision(dgCollision *collision);

    void SetGroupID(dgUnsigned32 id);
    void SetMatrix(const dgMatrix &matrix);
    void SetMatrixIgnoreSleep(const dgMatrix &matrix);
    void SetUserData(void *const userData);
    void SetForce(const dgVector &force);
    void SetTorque(const dgVector &torque);
    void SetOmega(const dgVector &omega);
    void SetVelocity(const dgVector &velocity);
    void SetLinearDamping(dgFloat32 linearDamp);
    void SetAngularDamping(const dgVector &angularDamp);
    void SetCentreOfMass(const dgVector &com);
    void SetAparentMassMatrix(const dgVector &massMatrix);
    void SetMassMatrix(dgFloat32 mass, dgFloat32 Ix, dgFloat32 Iy, dgFloat32 Iz);
    //  void SetGyroscopicTorqueMode (bool mode);
    void SetCollisionWithLinkedBodies(bool state);
    //  void SetFreezeTreshhold (dgFloat32 freezeAccel2, dgFloat32 freezeAlpha2, dgFloat32 freezeSpeed2, dgFloat32 freezeOmega2);

    void SetContinuesCollisionMode(bool mode);
    void SetDestructorCallback(OnBodyDestroy destructor);
    void SetMatrixUpdateCallback(OnMatrixUpdateCallback callback);
    OnMatrixUpdateCallback GetMatrixUpdateCallback() const;
    //  void SetAutoactiveNotify (OnActivation activate);
    void SetExtForceAndTorqueCallback(OnApplyExtForceAndTorque callback);
    OnApplyExtForceAndTorque GetExtForceAndTorqueCallback() const;

    dgConstraint *GetFirstJoint() const;
    dgConstraint *GetNextJoint(const dgConstraint *joint) const;

    dgConstraint *GetFirstContact() const;
    dgConstraint *GetNextContact(const dgConstraint *joint) const;

    void *GetUserData() const;
    dgWorld *GetWorld() const;
    const dgVector &GetMass() const;
    const dgVector &GetInvMass() const;
    const dgVector &GetAparentMass() const;

    const dgVector &GetOmega() const;
    const dgVector &GetVelocity() const;
    const dgVector &GetForce() const;
    const dgVector &GetTorque() const;
    const dgVector &GetNetForce() const;
    const dgVector &GetNetTorque() const;

    dgCollision *GetCollision() const;
    dgUnsigned32 GetGroupID() const;
    const dgMatrix &GetMatrix() const;
    const dgVector &GetPosition() const;
    const dgQuaternion &GetRotation() const;

    dgFloat32 GetLinearDamping() const;
    dgVector GetAngularDamping() const;
    dgVector GetCentreOfMass() const;
    bool IsInEquelibrium() const;

    void GetAABB(dgVector &p0, dgVector &p1) const;

    bool GetSleepState() const;
    bool GetAutoSleep() const;
    void SetAutoSleep(bool state);

    bool GetFreeze() const;
    void SetFreeze(bool state);

    void Freeze();
    void Unfreeze();

    dgInt32 GetUniqueID() const;

    bool GetCollisionWithLinkedBodies() const;
    bool GetContinuesCollisionMode() const;

    void AddBuoyancyForce(dgFloat32 fluidDensity, dgFloat32 fluidLinearViscousity, dgFloat32 fluidAngularViscousity,
                          const dgVector &gravityVector, GetBuoyancyPlane buoyancyPlane, void *const context);

    dgVector CalculateInverseDynamicForce(const dgVector &desiredVeloc, dgFloat32 timestep) const;

    //  dgFloat32 RayCast (const dgVector& globalP0, const dgVector& globalP1,
    dgFloat32 RayCast(const dgLineBox &line,
                      OnRayCastAction filter, OnRayPrecastAction preFilter, void *const userData, dgFloat32 minT) const;
    //  dgFloat32 RayCastSimd (const dgVector& globalP0, const dgVector& globalP1,
    //                     OnRayCastAction filter, OnRayPrecastAction preFilter, void* userData, dgFloat32 minT) const;

    void CalcInvInertiaMatrix();
    void CalcInvInertiaMatrixSimd();
    const dgMatrix &GetCollisionMatrix() const;

    dgBodyMasterList::dgListNode *GetMasterList() const;

    void InvalidateCache();

private:
    void SetMatrixOriginAndRotation(const dgMatrix &matrix);

    void CalculateContinueVelocity(dgFloat32 timestep, dgVector &veloc, dgVector &omega) const;
    void CalculateContinueVelocitySimd(dgFloat32 timestep, dgVector &veloc, dgVector &omega) const;

    dgVector GetTrajectory(const dgVector &veloc, const dgVector &omega) const;
    void IntegrateVelocity(dgFloat32 timestep);
    void UpdateMatrix(dgFloat32 timestep, dgInt32 threadIndex);
    void UpdateCollisionMatrix(dgFloat32 timestep, dgInt32 threadIndex);
    void UpdateCollisionMatrixSimd(dgFloat32 timestep, dgInt32 threadIndex);

    void ApplyExtenalForces(dgFloat32 timestep, dgInt32 threadIndex);
    void AddImpulse(const dgVector &pointVeloc, const dgVector &pointPosit);

    void ApplyImpulseArray(dgInt32 count, dgInt32 strideInBytes, const dgFloat32 *const impulseArray, const dgFloat32 *const pointArray);

    //  void AddGyroscopicTorque();
    void AddDamingAcceleration();

    dgMatrix CalculateInertiaMatrix() const;
    dgMatrix CalculateInvInertiaMatrix() const;

    dgMatrix m_matrix;
    dgMatrix m_collisionWorldMatrix;
    dgMatrix m_invWorldInertiaMatrix;
    dgQuaternion m_rotation;

    dgVector m_veloc;
    dgVector m_omega;
    dgVector m_accel;
    dgVector m_alpha;
    dgVector m_netForce;
    dgVector m_netTorque;
    dgVector m_prevExternalForce;
    dgVector m_prevExternalTorque;

    dgVector m_mass;
    dgVector m_invMass;
    dgVector m_aparentMass;
    dgVector m_localCentreOfMass;
    dgVector m_globalCentreOfMass;
    dgVector m_minAABB;
    dgVector m_maxAABB;
    dgVector m_dampCoef;

    dgInt32 m_index;
    dgInt32 m_uniqueID;
    dgInt32 m_bodyGroupId;
    dgInt32 m_genericLRUMark;
    dgInt32 m_sleepingCounter;
    dgUnsigned32 m_dynamicsLru;
    dgUnsigned32 m_isInDerstruionArrayLRU;

    dgUnsigned32 m_freeze : 1;
    dgUnsigned32 m_sleeping : 1;
    dgUnsigned32 m_autoSleep : 1;
    dgUnsigned32 m_isInWorld : 1;
    dgUnsigned32 m_equilibrium : 1;
    dgUnsigned32 m_continueCollisionMode : 1;
    dgUnsigned32 m_spawnnedFromCallback : 1;
    dgUnsigned32 m_collideWithLinkedBodies : 1;
    dgUnsigned32 m_solverInContinueCollision : 1;
    dgUnsigned32 m_inCallback : 1;

    void *m_userData;
    dgWorld *m_world;
    dgCollision *m_collision;
    dgBroadPhaseList m_collisionCell;
    dgBodyMasterList::dgListNode *m_masterNode;

    OnBodyDestroy m_destructor;
    OnMatrixUpdateCallback m_matrixUpdate;
    OnApplyExtForceAndTorque m_applyExtForces;

    void reset();

    friend class dgWorld;
    friend class dgContact;
    friend class dgCollision;
    friend class dgBodyChunk;
    friend class dgSortArray;
    friend class dgConstraint;
    friend class dgContactArray;
    friend class dgContactSolver;
    friend class dgBroadPhaseCell;
    friend class dgCollisionConvex;
    friend class dgCollisionEllipse;
    friend class dgCollisionCompound;
    friend class dgCollisionUserMesh;
    friend class dgWorldDynamicUpdate;
    friend class dgCollisionConvexHull;
    friend class dgCollisionScene;
    friend class dgCollisionBVH;
    friend class dgBodyMasterList;
    friend class dgJacobianMemory;
    friend class dgBilateralConstraint;
    friend class dgBroadPhaseCollision;
    friend class dgSolverWorlkerThreads;
    friend class dgCollisionConvexModifier;
    friend class dgCollidingPairCollector;
    friend class dgAABBOverlapPairList;
    friend class dgParallelSolverClear;
    friend class dgParallelSolverUpdateForce;
    friend class dgParallelSolverUpdateVeloc;
    friend class dgParallelSolverBodyInertia;
    friend class dgBroadPhaseApplyExternalForce;
    friend class dgParallelSolverBuildJacobianRows;
    friend class dgParallelSolverBuildJacobianMatrix;
} DG_GCC_VECTOR_ALIGMENT;

// *****************************************************************************
//
//   Implementation
//
// *****************************************************************************

inline void dgBody::reset() {
    m_matrix = dgGetZeroMatrix();
    m_collisionWorldMatrix = dgGetZeroMatrix();
    m_invWorldInertiaMatrix = dgGetZeroMatrix();
    m_rotation = dgQuaternion(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));

    m_veloc = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_omega = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_accel = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_alpha = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_netForce = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_netTorque = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    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));

    m_mass = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_invMass = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_aparentMass = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_localCentreOfMass = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_globalCentreOfMass = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_minAABB = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_maxAABB = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_dampCoef = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));

    m_index = 0;
    m_uniqueID = 0;
    m_bodyGroupId = 0;
    m_genericLRUMark = 0;
    m_sleepingCounter = 0;
    m_dynamicsLru = 0;
    m_isInDerstruionArrayLRU = 0;

    m_freeze = 0;
    m_sleeping = 0;
    m_autoSleep = 0;
    m_isInWorld = 0;
    m_equilibrium = 0;
    m_continueCollisionMode = 0;
    m_spawnnedFromCallback = 0;
    m_collideWithLinkedBodies = 0;
    m_solverInContinueCollision = 0;
    m_inCallback = 0;

    m_userData = NULL;
    m_world = NULL;
    m_collision = NULL;
    m_collisionCell.reset();
    m_masterNode = NULL;

    m_destructor = NULL;
    m_matrixUpdate = NULL;
    m_applyExtForces = NULL;
}

inline void dgBody::SetAutoSleep(bool state) {
    m_autoSleep = dgUnsigned32(state);
    if (m_autoSleep == 0) {
        m_sleeping = false;
    }
}

inline bool dgBody::GetAutoSleep() const {
    return m_autoSleep;
}

inline bool dgBody::GetSleepState() const {
    return m_sleeping;
}

/*
inline bool dgBody::GetActive () const
{
    return m_active;
}
*/

inline bool dgBody::GetCollisionWithLinkedBodies() const {
    return m_collideWithLinkedBodies;
}

inline void dgBody::SetCollisionWithLinkedBodies(bool state) {
    m_collideWithLinkedBodies = dgUnsigned32(state);
}

inline void dgBody::SetUserData(void *const userData) {
    m_userData = userData;
}

inline void *dgBody::GetUserData() const {
    return m_userData;
}

inline dgWorld *dgBody::GetWorld() const {
    return m_world;
}

inline dgUnsigned32 dgBody::GetGroupID() const {
    return dgUnsigned32(m_bodyGroupId);
}

inline void dgBody::SetGroupID(dgUnsigned32 id) {
    m_bodyGroupId = dgInt32(id);
}

inline void dgBody::SetDestructorCallback(OnBodyDestroy destructor) {
    m_destructor = destructor;
}

inline void dgBody::SetExtForceAndTorqueCallback(OnApplyExtForceAndTorque callback) {
    m_applyExtForces = callback;
}

inline OnApplyExtForceAndTorque dgBody::GetExtForceAndTorqueCallback() const {
    return m_applyExtForces;
}

/*
inline void dgBody::SetAutoactiveNotify (OnActivation activate)
{
    m_activation = activate;
    if (m_activation) {
        m_activation (*this, m_active ? 1 : 0);
    }
}
*/

inline void dgBody::SetMatrixUpdateCallback(OnMatrixUpdateCallback callback) {
    m_matrixUpdate = callback;
}

inline OnMatrixUpdateCallback dgBody::GetMatrixUpdateCallback() const {
    return m_matrixUpdate;
}

/*
inline void dgBody::SetFreezeTreshhold (dgFloat32 freezeAccel2, dgFloat32 freezeAlpha2, dgFloat32 freezeSpeed2, dgFloat32 freezeOmega2)
{
    m_freezeAccel2 = GetMax (freezeAccel2, dgFloat32(DG_FREEZE_MAG2));
    m_freezeAlpha2 = GetMax (freezeAlpha2, dgFloat32(DG_FREEZE_MAG2));
    m_freezeSpeed2 = GetMax (freezeSpeed2, dgFloat32(DG_FREEZE_MAG2));
    m_freezeOmega2 = GetMax (freezeOmega2, dgFloat32(DG_FREEZE_MAG2));
}

inline void dgBody::GetFreezeTreshhold (dgFloat32& freezeAccel2, dgFloat32& freezeAlpha2, dgFloat32& freezeSpeed2, dgFloat32& freezeOmega2) const
{
    freezeAccel2 = m_freezeAccel2;
    freezeAlpha2 = m_freezeAlpha2;
    freezeSpeed2 = m_freezeSpeed2;
    freezeOmega2 = m_freezeOmega2;
}
*/

inline void dgBody::SetOmega(const dgVector &omega) {
    m_omega = omega;
}

inline void dgBody::SetVelocity(const dgVector &velocity) {
    m_veloc = velocity;
}

inline void dgBody::SetCentreOfMass(const dgVector &com) {
    m_localCentreOfMass.m_x = com.m_x;
    m_localCentreOfMass.m_y = com.m_y;
    m_localCentreOfMass.m_z = com.m_z;
    m_localCentreOfMass.m_w = dgFloat32(1.0f);
    m_globalCentreOfMass = m_matrix.TransformVector(m_localCentreOfMass);
}

inline void dgBody::AddForce(const dgVector &force) {
    SetForce(m_accel + force);
}

inline void dgBody::AddTorque(const dgVector &torque) {
    SetTorque(torque + m_alpha);
}

inline const dgVector &dgBody::GetMass() const {
    return m_mass;
}

inline const dgVector &dgBody::GetAparentMass() const {
    return m_aparentMass;
}

inline const dgVector &dgBody::GetInvMass() const {
    return m_invMass;
}

inline const dgVector &dgBody::GetOmega() const {
    return m_omega;
}

inline const dgVector &dgBody::GetVelocity() const {
    return m_veloc;
}

inline const dgVector &dgBody::GetForce() const {
    return m_accel;
}

inline const dgVector &dgBody::GetTorque() const {
    return m_alpha;
}

inline const dgVector &dgBody::GetNetForce() const {
    return m_netForce;
}

inline const dgVector &dgBody::GetNetTorque() const {
    return m_netTorque;
}

inline dgCollision *dgBody::GetCollision() const {
    return m_collision;
}

inline const dgVector &dgBody::GetPosition() const {
    return m_matrix.m_posit;
}

inline const dgQuaternion &dgBody::GetRotation() const {
    return m_rotation;
}

inline const dgMatrix &dgBody::GetMatrix() const {
    return m_matrix;
}

inline dgVector dgBody::GetCentreOfMass() const {
    return m_localCentreOfMass;
}

inline void dgBody::GetAABB(dgVector &p0, dgVector &p1) const {
    p0.m_x = m_minAABB.m_x;
    p0.m_y = m_minAABB.m_y;
    p0.m_z = m_minAABB.m_z;
    p1.m_x = m_maxAABB.m_x;
    p1.m_y = m_maxAABB.m_y;
    p1.m_z = m_maxAABB.m_z;
}

/*
inline void dgBody::SetGyroscopicTorqueMode (bool mode)
{
    m_applyGyroscopic = mode;
}

inline bool dgBody::GetGyroscopicTorqueMode () const
{
    return m_applyGyroscopic;
}
*/

inline const dgMatrix &dgBody::GetCollisionMatrix() const {
    return m_collisionWorldMatrix;
}

inline void dgBody::SetContinuesCollisionMode(bool mode) {
    m_continueCollisionMode = dgUnsigned32(mode);
}

inline bool dgBody::GetContinuesCollisionMode() const {
    return m_continueCollisionMode;
}

inline void dgBody::ApplyExtenalForces(dgFloat32 timestep, dgInt32 threadIndex) {
    m_accel = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    m_alpha = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    if (m_applyExtForces) {
        m_applyExtForces(reinterpret_cast<NewtonBody *>(this), timestep, threadIndex);
    }
}

inline dgFloat32 dgBody::GetLinearDamping() const {
    //  return (m_linearDampCoef - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT);
    return (m_dampCoef.m_w - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT);
}

inline dgVector dgBody::GetAngularDamping() const {
    return dgVector((m_dampCoef.m_x - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT),
                    (m_dampCoef.m_y - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT),
                    (m_dampCoef.m_z - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT), dgFloat32(0.0f));
}

inline void dgBody::SetLinearDamping(dgFloat32 linearDamp) {
    linearDamp = ClampValue(linearDamp, dgFloat32(0.0f), dgFloat32(1.0f));
    m_dampCoef.m_w = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * linearDamp;
}

inline void dgBody::SetAngularDamping(const dgVector &angularDamp) {
    dgFloat32 tmp;

    tmp = ClampValue(angularDamp.m_x, dgFloat32(0.0f), dgFloat32(1.0f));
    m_dampCoef.m_x = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * tmp;

    tmp = ClampValue(angularDamp.m_y, dgFloat32(0.0f), dgFloat32(1.0f));
    m_dampCoef.m_y = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * tmp;

    tmp = ClampValue(angularDamp.m_z, dgFloat32(0.0f), dgFloat32(1.0f));
    m_dampCoef.m_z = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * tmp;
}

inline void dgBody::AddDamingAcceleration() {
    m_veloc -= m_veloc.Scale(m_dampCoef.m_w);
    dgVector omega(m_matrix.UnrotateVector(m_omega));
    omega -= omega.CompProduct(m_dampCoef);
    m_omega = m_matrix.RotateVector(omega);
}

/*
inline void dgBody::AddGyroscopicTorque()
{
    NEWTON_ASSERT (0);
    if (m_applyGyroscopic) {
        const dgVector inertia = m_mass;
        dgVector omega (m_matrix.UnrotateVector (m_omega));
        m_alpha -= m_matrix.RotateVector(omega.CompProduct(inertia) * omega);
    }
}
*/

inline void dgBody::SetForce(const dgVector &force) {
    dgFloat32 errMag2;
    dgVector error;

    m_accel = force;
    error = m_accel - m_prevExternalForce;
    errMag2 = (error % error) * m_invMass[3] * m_invMass[3];
    if (errMag2 > DG_ErrTolerance2) {
        m_sleepingCounter = 0;
    }
}

inline void dgBody::SetTorque(const dgVector &torque) {
    dgFloat32 errMag2;
    dgVector error;

    m_alpha = torque;
    error = m_alpha - m_prevExternalTorque;
    errMag2 = (error % error) * m_invMass[3] * m_invMass[3];
    if (errMag2 > DG_ErrTolerance2) {
        m_sleepingCounter = 0;
    }
}

// inline int dgBody::GetApplicationFreezeState() const
//{
//  return m_aplycationFreeze ? 1 : 0;
// }
// inline void dgBody::SetApplicationFreezeState(dgInt32 state)
//{
//  m_aplycationFreeze = state ? true : false;
// }

inline dgBodyMasterList::dgListNode *dgBody::GetMasterList() const {
    return m_masterNode;
}

inline bool dgBody::GetFreeze() const {
    return m_freeze;
}

inline dgInt32 dgBody::GetUniqueID() const {
    return m_uniqueID;
}

inline bool dgBody::IsInEquelibrium() const {
    dgFloat32 invMassMag2 = m_invMass[3] * m_invMass[3];
    if (m_equilibrium) {
        dgVector error(m_accel - m_prevExternalForce);
        dgFloat32 errMag2 = (error % error) * invMassMag2;
        if (errMag2 < DG_ErrTolerance2) {
            error = m_alpha - m_prevExternalTorque;
            errMag2 = (error % error) * invMassMag2;
            if (errMag2 < DG_ErrTolerance2) {
                errMag2 = (m_netForce % m_netForce) * invMassMag2;
                if (errMag2 < DG_ErrTolerance2) {
                    errMag2 = (m_netTorque % m_netTorque) * invMassMag2;
                    if (errMag2 < DG_ErrTolerance2) {
                        errMag2 = m_veloc % m_veloc;
                        if (errMag2 < DG_ErrTolerance2) {
                            errMag2 = m_omega % m_omega;
                            if (errMag2 < DG_ErrTolerance2) {
                                return true;
                            }
                        }
                    }
                }
            }
        }
    }

    return false;
}

inline void dgBody::SetMatrixOriginAndRotation(const dgMatrix &matrix) {
    m_matrix = matrix;

#ifdef _DEBUG
    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 (dgInt32 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

    m_rotation = dgQuaternion(m_matrix);
    m_globalCentreOfMass = m_matrix.TransformVector(m_localCentreOfMass);

    //  matrix.m_front = matrix.m_front.Scale (dgRsqrt (matrix.m_front % matrix.m_front));
    //  matrix.m_right = matrix.m_front * matrix.m_up;
    //  matrix.m_right = matrix.m_right.Scale (dgRsqrt (matrix.m_right % matrix.m_right));
    //  matrix.m_up = matrix.m_right * matrix.m_front;
}

#endif // !defined(AFX_DGBODY_H__C16EDCD6_53C4_4C6F_A70A_591819F7187E__INCLUDED_)