My Project: dynamics.mod/NewtonDynamics/source/physics/dgDynamicBody.h Source File

My Project

dgDynamicBody.h
Go to the documentation of this file.
1 /* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21
22 #ifndef _DG_DYNAMIC_BODY_H_
23 #define _DG_DYNAMIC_BODY_H_
24
25 #include "dgPhysicsStdafx.h"
26 #include "dgBody.h"
27
28 #define DG_MIN_SPEED_ATT dgFloat32(0.0f)
29 #define DG_MAX_SPEED_ATT dgFloat32(0.02f)
30 #define DG_FREEZE_MAG dgFloat32(0.1f)
31 #define DG_FREEZE_MAG2 dgFloat32(DG_FREEZE_MAG * DG_FREEZE_MAG)
32
33 #define DG_ErrTolerance (1.0e-2f)
34 #define DG_ErrTolerance2 (DG_ErrTolerance * DG_ErrTolerance)
35
36
37
38 //DG_MSC_VECTOR_ALIGMENT
39 DG_MSC_VECTOR_ALIGMENT
40 class dgDynamicBody : public dgBody
41 {
42 public:
43 dgDynamicBody();
44 dgDynamicBody (dgWorld* const world, const dgTree<const dgCollision*, dgInt32>* const collisionNode, dgDeserialize serializeCallback, void* const userData);
45 virtual ~dgDynamicBody ();
46
47 virtual const dgVector& GetForce() const;
48 virtual const dgVector& GetTorque() const;
49
50 virtual void AddForce (const dgVector& force);
51 virtual void AddTorque (const dgVector& torque);
52 virtual void SetForce (const dgVector& force);
53 virtual void SetTorque (const dgVector& torque);
54
55 virtual dgFloat32 GetLinearDamping () const;
56 virtual dgVector GetAngularDamping () const;
57 virtual void SetLinearDamping (dgFloat32 linearDamp);
58 virtual void SetAngularDamping (const dgVector& angularDamp);
59
60 virtual void AttachCollision (dgCollisionInstance* const collision);
61 virtual dgVector PredictLinearVelocity(dgFloat32 timestep) const;
62 virtual dgVector PredictAngularVelocity(dgFloat32 timestep) const;
63
64 virtual void InvalidateCache();
65 virtual void SetMatrixIgnoreSleep(const dgMatrix& matrix);
66
67 virtual bool IsInEquilibrium () const;
68 virtual void SetCollidable (bool state) {}
69
70 virtual void ApplyExtenalForces (dgFloat32 timestep, dgInt32 threadIndex);
71 virtual OnApplyExtForceAndTorque GetExtForceAndTorqueCallback () const;
72 virtual void SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback);
73
74 virtual void Serialize (const dgTree<dgInt32, const dgCollision*>* const collisionNode, dgSerialize serializeCallback, void* const userData);
75
76 private:
77 virtual void AddDampingAcceleration();
78
79 dgVector m_accel;
80 dgVector m_alpha;
81 dgVector m_prevExternalForce;
82 dgVector m_prevExternalTorque;
83 dgVector m_dampCoef;
84 dgVector m_aparentMass;
85 dgInt32 m_sleepingCounter;
86 dgUnsigned32 m_isInDestructionArrayLRU;
87
88 static dgVector m_equilibriumError2;
89
90 OnApplyExtForceAndTorque m_applyExtForces;
91
92 friend class dgWorld;
93 friend class dgBroadPhase;
94 friend class dgAmpInstance;
95 friend class dgBodyMasterList;
96 friend class dgWorldDynamicUpdate;
97 } DG_GCC_VECTOR_ALIGMENT;
98
99
100 DG_INLINE const dgVector& dgDynamicBody::GetForce() const
101 {
102 return m_accel;
103 }
104
105 DG_INLINE const dgVector& dgDynamicBody::GetTorque() const
106 {
107 return m_alpha;
108 }
109
110
111
112
113
114 DG_INLINE dgFloat32 dgDynamicBody::GetLinearDamping () const
115 {
116 return (m_dampCoef.m_w - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT);
117 }
118
119 DG_INLINE dgVector dgDynamicBody::GetAngularDamping () const
120 {
121 return dgVector ((m_dampCoef.m_x - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT),
122 (m_dampCoef.m_y - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT),
123 (m_dampCoef.m_z - DG_MIN_SPEED_ATT) / (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT), dgFloat32 (0.0f));
124 }
125
126 DG_INLINE void dgDynamicBody::SetLinearDamping (dgFloat32 linearDamp)
127 {
128 linearDamp = dgClamp (linearDamp, dgFloat32(0.0f), dgFloat32(1.0f));
129 m_dampCoef.m_w = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * linearDamp;
130 }
131
132 DG_INLINE void dgDynamicBody::SetAngularDamping (const dgVector& angularDamp)
133 {
134 dgFloat32 tmp = dgClamp (angularDamp.m_x, dgFloat32(0.0f), dgFloat32(1.0f));
135 m_dampCoef.m_x = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * tmp;
136
137 tmp = dgClamp (angularDamp.m_y, dgFloat32(0.0f), dgFloat32(1.0f));
138 m_dampCoef.m_y = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * tmp;
139
140 tmp = dgClamp (angularDamp.m_z, dgFloat32(0.0f), dgFloat32(1.0f));
141 m_dampCoef.m_z = DG_MIN_SPEED_ATT + (DG_MAX_SPEED_ATT - DG_MIN_SPEED_ATT) * tmp;
142 }
143
144
145
146 DG_INLINE void dgDynamicBody::AddForce (const dgVector& force)
147 {
148 SetForce (m_accel + force);
149 }
150
151 DG_INLINE void dgDynamicBody::AddTorque (const dgVector& torque)
152 {
153 SetTorque (torque + m_alpha);
154 }
155
156
157 DG_INLINE void dgDynamicBody::SetForce (const dgVector& force)
158 {
159 m_accel = force;
160 dgVector error (m_accel - m_prevExternalForce);
161 dgFloat32 errMag2 = (error % error) * m_invMass[3] * m_invMass[3];
162 if (errMag2 > DG_ErrTolerance2) {
163 m_sleepingCounter = 0;
164 m_equilibrium = false;
165 }
166 }
167
168 DG_INLINE void dgDynamicBody::SetTorque (const dgVector& torque)
169 {
170 m_alpha = torque;
171 dgVector error (m_alpha - m_prevExternalTorque);
172 dgFloat32 errMag2 = (error % error) * m_invMass[3] * m_invMass[3];
173 if (errMag2 > DG_ErrTolerance2) {
174 m_sleepingCounter = 0;
175 m_equilibrium = false;
176 }
177 }
178
179 DG_INLINE void dgDynamicBody::AddDampingAcceleration()
180 {
181 m_veloc -= m_veloc.Scale4 (m_dampCoef.m_w);
182 dgVector omega (m_matrix.UnrotateVector (m_omega));
183 omega -= omega.CompProduct4 (m_dampCoef);
184 m_omega = m_matrix.RotateVector (omega);
185 }
186
187
188 DG_INLINE dgVector dgDynamicBody::PredictLinearVelocity(dgFloat32 timestep) const
189 {
190 return m_veloc + m_accel.Scale3 (timestep * m_invMass.m_w);
191 }
192
193 DG_INLINE dgVector dgDynamicBody::PredictAngularVelocity(dgFloat32 timestep) const
194 {
195 return m_omega + m_invWorldInertiaMatrix.RotateVector(m_alpha).Scale3 (timestep);
196 }
197
198
199 DG_INLINE dgBody::OnApplyExtForceAndTorque dgDynamicBody::GetExtForceAndTorqueCallback () const
200 {
201 return m_applyExtForces;
202 }
203
204 DG_INLINE void dgDynamicBody::SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback)
205 {
206 m_applyExtForces = callback;
207 }
208
209 #endif
210
211
dgDynamicBody::AddForce
virtual void AddForce(const dgVector &force)
Definition: dgDynamicBody.h:146
dgDynamicBody::ApplyExtenalForces
virtual void ApplyExtenalForces(dgFloat32 timestep, dgInt32 threadIndex)
Definition: dgDynamicBody.cpp:132
dgAmpInstance
Definition: dgAmpInstance.h:143
dgBodyMasterList
Definition: dgBodyMasterList.h:57
dgDynamicBody::dgDynamicBody
dgDynamicBody()
Definition: dgDynamicBody.cpp:34
DG_GCC_VECTOR_ALIGMENT
dgDynamicBody DG_GCC_VECTOR_ALIGMENT
dgMatrix::UnrotateVector
dgVector UnrotateVector(const dgVector &v) const
Definition: dgMatrix.h:206
dgBody
Definition: dgBody.h:44
dgDynamicBody::PredictLinearVelocity
virtual dgVector PredictLinearVelocity(dgFloat32 timestep) const
Definition: dgDynamicBody.h:188
dgDynamicBody::IsInEquilibrium
virtual bool IsInEquilibrium() const
Definition: dgDynamicBody.cpp:110
userData
void *const userData
Definition: dgBroadPhase.h:35
DG_INLINE
#define DG_INLINE
Definition: dgTypes.h:178
dgFloat32
float dgFloat32
Definition: dgTypes.h:233
dgInt32
int dgInt32
Definition: dgTypes.h:218
dgBody.h
dgVector
Definition: dgVector.h:639
dgDynamicBody::SetTorque
virtual void SetTorque(const dgVector &torque)
Definition: dgDynamicBody.h:168
dgDynamicBody::GetLinearDamping
virtual dgFloat32 GetLinearDamping() const
Definition: dgDynamicBody.h:114
dgBody::m_omega
dgVector m_omega
Definition: dgBody.h:209
DG_ErrTolerance2
#define DG_ErrTolerance2
Definition: dgDynamicBody.h:34
dgVector::m_z
dgFloat32 m_z
Definition: dgVector.h:1027
dgVector::Scale3
DG_INLINE dgVector Scale3(dgFloat32 s) const
Definition: dgVector.h:724
DG_MAX_SPEED_ATT
#define DG_MAX_SPEED_ATT
Definition: dgDynamicBody.h:29
dgDynamicBody::AddTorque
virtual void AddTorque(const dgVector &torque)
Definition: dgDynamicBody.h:151
dgMatrix::RotateVector
dgVector RotateVector(const dgVector &v) const
Definition: dgMatrix.h:200
dgDynamicBody::GetTorque
virtual const dgVector & GetTorque() const
Definition: dgDynamicBody.h:105
dgClamp
DG_INLINE T dgClamp(T val, T min, T max)
Definition: dgTypes.h:317
dgDynamicBody::~dgDynamicBody
virtual ~dgDynamicBody()
Definition: dgDynamicBody.cpp:74
dgWorldDynamicUpdate
Definition: dgWorldDynamicUpdate.h:252
dgSerialize
void(dgApi * dgSerialize)(void *const userData, const void *const buffer, size_t size)
Definition: dgTypes.h:280
dgTree
Definition: dgTree.h:88
dgDynamicBody::SetExtForceAndTorqueCallback
virtual void SetExtForceAndTorqueCallback(OnApplyExtForceAndTorque callback)
Definition: dgDynamicBody.h:204
DG_MSC_VECTOR_ALIGMENT
#define DG_MSC_VECTOR_ALIGMENT
Definition: dgTypes.h:194
dgBody::m_matrix
dgMatrix m_matrix
Definition: dgBody.h:204
dgVector::m_x
dgFloat32 m_x
Definition: dgVector.h:1025
dgDynamicBody::SetAngularDamping
virtual void SetAngularDamping(const dgVector &angularDamp)
Definition: dgDynamicBody.h:132
dgBroadPhase
Definition: dgBroadPhase.h:65
dgDynamicBody::PredictAngularVelocity
virtual dgVector PredictAngularVelocity(dgFloat32 timestep) const
Definition: dgDynamicBody.h:193
dgDeserialize
void(dgApi * dgDeserialize)(void *const userData, void *buffer, size_t size)
Definition: dgTypes.h:279
dgDynamicBody::SetMatrixIgnoreSleep
virtual void SetMatrixIgnoreSleep(const dgMatrix &matrix)
Definition: dgDynamicBody.cpp:92
dgDynamicBody::GetAngularDamping
virtual dgVector GetAngularDamping() const
Definition: dgDynamicBody.h:119
dgCollisionInstance
Definition: dgCollisionInstance.h:30
dgBody::m_invWorldInertiaMatrix
dgMatrix m_invWorldInertiaMatrix
Definition: dgBody.h:203
dgDynamicBody::InvalidateCache
virtual void InvalidateCache()
Definition: dgDynamicBody.cpp:167
dgDynamicBody::Serialize
virtual void Serialize(const dgTree< dgInt32, const dgCollision *> *const collisionNode, dgSerialize serializeCallback, void *const userData)
Definition: dgDynamicBody.cpp:82
dgDynamicBody::AttachCollision
virtual void AttachCollision(dgCollisionInstance *const collision)
Definition: dgDynamicBody.cpp:101
dgUnsigned32
unsigned dgUnsigned32
Definition: dgTypes.h:219
DG_MIN_SPEED_ATT
#define DG_MIN_SPEED_ATT
Definition: dgDynamicBody.h:28
dgVector::CompProduct4
DG_INLINE dgVector CompProduct4(const dgVector &A) const
Definition: dgVector.h:851
dgBody::m_veloc
dgVector m_veloc
Definition: dgBody.h:208
dgDynamicBody::SetCollidable
virtual void SetCollidable(bool state)
Definition: dgDynamicBody.h:68
threadIndex
dgFloat32 dgInt32 threadIndex
Definition: dgConstraint.h:44
timestep
dgFloat32 timestep
Definition: dgBallConstraint.h:34
dgMatrix
Definition: dgMatrix.h:40
dgPhysicsStdafx.h
dgDynamicBody::GetForce
virtual const dgVector & GetForce() const
Definition: dgDynamicBody.h:100
dgBody::m_equilibrium
dgUnsigned32 m_equilibrium
Definition: dgBody.h:233
dgDynamicBody::GetExtForceAndTorqueCallback
virtual OnApplyExtForceAndTorque GetExtForceAndTorqueCallback() const
Definition: dgDynamicBody.h:199
dgWorld
Definition: dgWorld.h:129
dgDynamicBody::SetLinearDamping
virtual void SetLinearDamping(dgFloat32 linearDamp)
Definition: dgDynamicBody.h:126
dgDynamicBody::SetForce
virtual void SetForce(const dgVector &force)
Definition: dgDynamicBody.h:157
dgVector::m_w
dgFloat32 m_w
Definition: dgVector.h:1028
dgDynamicBody
Definition: dgDynamicBody.h:40
dgBody::OnApplyExtForceAndTorque
void(dgApi * OnApplyExtForceAndTorque)(dgBody &me, dgFloat32 timestep, dgInt32 threadIndex)
Definition: dgBody.h:48
dgBody::m_invMass
dgVector m_invMass
Definition: dgBody.h:207
dgVector::Scale4
DG_INLINE dgVector Scale4(dgFloat32 s) const
Definition: dgVector.h:730
dgVector::m_y
dgFloat32 m_y
Definition: dgVector.h:1026