/* Copyright (c) <2003-2011> * * 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 "dgPhysicsStdafx.h" #include "dgBodyMasterList.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) struct dgLineBox { dgVector m_l0; dgVector m_l1; dgVector m_boxL0; dgVector m_boxL1; }; 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) (dgBody& me); typedef void (dgApi *OnApplyExtForceAndTorque) (dgBody& me, dgFloat32 timestep, dgInt32 threadIndex); typedef void (dgApi *OnMatrixUpdateCallback) (const dgBody& me, const dgMatrix& matrix, dgInt32 threadIndex); typedef dgUnsigned32 (dgApi *OnRayPrecastAction) (const dgBody* const body, const dgCollision* const collision, void* const userData); typedef dgFloat32 (dgApi *OnRayCastAction) (const dgBody* const body, const dgVector& normal, dgInt32 collisionID, void* const userData, dgFloat32 intersetParam); typedef dgUnsigned32 (dgApi *GetBuoyancyPlane) (void* collisionID, void* context, const dgMatrix& matrix, dgPlane& plane); #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]; }; 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 (); // void SetAutoactiveNotify (OnActivation activate); void SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback); OnApplyExtForceAndTorque GetExtForceAndTorqueCallback () const; dgConstraint* GetFirstJoint() const; dgConstraint* GetNextJoint(dgConstraint* joint) const; dgConstraint* GetFirstContact() const; dgConstraint* GetNextContact(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 AddGyroscopicTorque(); void AddDamingAcceleration(); 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; 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::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 () { 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(*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() { _ASSERTE (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 errMag2; dgFloat32 invMassMag2; dgVector error; invMassMag2 = m_invMass[3] * m_invMass[3]; if (m_equilibrium) { error = m_accel - m_prevExternalForce; 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 ++) { _ASSERTE (dgCheckFloat(m_matrix[i][j])); } } int j0 = 1; int j1 = 2; for (dgInt32 i = 0; i < 3; i ++) { dgFloat32 val; _ASSERTE (m_matrix[i][3] == 0.0f); val = m_matrix[i] % m_matrix[i]; _ASSERTE (dgAbsf (val - 1.0f) < 1.0e-5f); dgVector tmp (m_matrix[j0] * m_matrix[j1]); val = tmp % m_matrix[i]; _ASSERTE (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_)