Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef BT_SOLVER_BODY_H
00017 #define BT_SOLVER_BODY_H
00018
00019 class btRigidBody;
00020 #include "LinearMath/btVector3.h"
00021 #include "LinearMath/btMatrix3x3.h"
00022
00023 #include "LinearMath/btAlignedAllocator.h"
00024 #include "LinearMath/btTransformUtil.h"
00025
00027 #ifdef BT_USE_SSE
00028 #define USE_SIMD 1
00029 #endif //
00030
00031
00032 #ifdef USE_SIMD
00033
00034 struct btSimdScalar
00035 {
00036 SIMD_FORCE_INLINE btSimdScalar()
00037 {
00038
00039 }
00040
00041 SIMD_FORCE_INLINE btSimdScalar(float fl)
00042 :m_vec128 (_mm_set1_ps(fl))
00043 {
00044 }
00045
00046 SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
00047 :m_vec128(v128)
00048 {
00049 }
00050 union
00051 {
00052 __m128 m_vec128;
00053 float m_floats[4];
00054 int m_ints[4];
00055 btScalar m_unusedPadding;
00056 };
00057 SIMD_FORCE_INLINE __m128 get128()
00058 {
00059 return m_vec128;
00060 }
00061
00062 SIMD_FORCE_INLINE const __m128 get128() const
00063 {
00064 return m_vec128;
00065 }
00066
00067 SIMD_FORCE_INLINE void set128(__m128 v128)
00068 {
00069 m_vec128 = v128;
00070 }
00071
00072 SIMD_FORCE_INLINE operator __m128()
00073 {
00074 return m_vec128;
00075 }
00076 SIMD_FORCE_INLINE operator const __m128() const
00077 {
00078 return m_vec128;
00079 }
00080
00081 SIMD_FORCE_INLINE operator float() const
00082 {
00083 return m_floats[0];
00084 }
00085
00086 };
00087
00089 SIMD_FORCE_INLINE btSimdScalar
00090 operator*(const btSimdScalar& v1, const btSimdScalar& v2)
00091 {
00092 return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
00093 }
00094
00096 SIMD_FORCE_INLINE btSimdScalar
00097 operator+(const btSimdScalar& v1, const btSimdScalar& v2)
00098 {
00099 return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
00100 }
00101
00102
00103 #else
00104 #define btSimdScalar btScalar
00105 #endif
00106
00108 ATTRIBUTE_ALIGNED64 (struct) btSolverBody
00109 {
00110 BT_DECLARE_ALIGNED_ALLOCATOR();
00111 btTransform m_worldTransform;
00112 btVector3 m_deltaLinearVelocity;
00113 btVector3 m_deltaAngularVelocity;
00114 btVector3 m_angularFactor;
00115 btVector3 m_linearFactor;
00116 btVector3 m_invMass;
00117 btVector3 m_pushVelocity;
00118 btVector3 m_turnVelocity;
00119 btVector3 m_linearVelocity;
00120 btVector3 m_angularVelocity;
00121
00122 btRigidBody* m_originalBody;
00123 void setWorldTransform(const btTransform& worldTransform)
00124 {
00125 m_worldTransform = worldTransform;
00126 }
00127
00128 const btTransform& getWorldTransform() const
00129 {
00130 return m_worldTransform;
00131 }
00132
00133 SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
00134 {
00135 if (m_originalBody)
00136 velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
00137 else
00138 velocity.setValue(0,0,0);
00139 }
00140
00141 SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
00142 {
00143 if (m_originalBody)
00144 angVel =m_angularVelocity+m_deltaAngularVelocity;
00145 else
00146 angVel.setValue(0,0,0);
00147 }
00148
00149
00150
00151 SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
00152 {
00153 if (m_originalBody)
00154 {
00155 m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
00156 m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
00157 }
00158 }
00159
00160 SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
00161 {
00162 if (m_originalBody)
00163 {
00164 m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
00165 m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
00166 }
00167 }
00168
00169
00170
00171 const btVector3& getDeltaLinearVelocity() const
00172 {
00173 return m_deltaLinearVelocity;
00174 }
00175
00176 const btVector3& getDeltaAngularVelocity() const
00177 {
00178 return m_deltaAngularVelocity;
00179 }
00180
00181 const btVector3& getPushVelocity() const
00182 {
00183 return m_pushVelocity;
00184 }
00185
00186 const btVector3& getTurnVelocity() const
00187 {
00188 return m_turnVelocity;
00189 }
00190
00191
00194
00195 btVector3& internalGetDeltaLinearVelocity()
00196 {
00197 return m_deltaLinearVelocity;
00198 }
00199
00200 btVector3& internalGetDeltaAngularVelocity()
00201 {
00202 return m_deltaAngularVelocity;
00203 }
00204
00205 const btVector3& internalGetAngularFactor() const
00206 {
00207 return m_angularFactor;
00208 }
00209
00210 const btVector3& internalGetInvMass() const
00211 {
00212 return m_invMass;
00213 }
00214
00215 void internalSetInvMass(const btVector3& invMass)
00216 {
00217 m_invMass = invMass;
00218 }
00219
00220 btVector3& internalGetPushVelocity()
00221 {
00222 return m_pushVelocity;
00223 }
00224
00225 btVector3& internalGetTurnVelocity()
00226 {
00227 return m_turnVelocity;
00228 }
00229
00230 SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
00231 {
00232 velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
00233 }
00234
00235 SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
00236 {
00237 angVel = m_angularVelocity+m_deltaAngularVelocity;
00238 }
00239
00240
00241
00242 SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
00243 {
00244 if (m_originalBody)
00245 {
00246 m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
00247 m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
00248 }
00249 }
00250
00251
00252
00253
00254 void writebackVelocity()
00255 {
00256 if (m_originalBody)
00257 {
00258 m_linearVelocity +=m_deltaLinearVelocity;
00259 m_angularVelocity += m_deltaAngularVelocity;
00260
00261
00262 }
00263 }
00264
00265
00266 void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
00267 {
00268 (void) timeStep;
00269 if (m_originalBody)
00270 {
00271 m_linearVelocity += m_deltaLinearVelocity;
00272 m_angularVelocity += m_deltaAngularVelocity;
00273
00274
00275 btTransform newTransform;
00276 if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
00277 {
00278
00279 btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
00280 m_worldTransform = newTransform;
00281 }
00282
00283
00284 }
00285 }
00286
00287
00288
00289 };
00290
00291 #endif //BT_SOLVER_BODY_H
00292
00293