Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/minigame4DHS14/src/external/bullet/LinearMath/btTransformUtil.h @ 10110

Last change on this file since 10110 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 7.1 KB
Line 
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15
16#ifndef BT_TRANSFORM_UTIL_H
17#define BT_TRANSFORM_UTIL_H
18
19#include "btTransform.h"
20#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
21
22
23
24
25SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
26{
27        return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
28      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
29      supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 
30}
31
32
33
34
35
36
37/// Utils related to temporal transforms
38class btTransformUtil
39{
40
41public:
42
43        static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
44        {
45                predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
46//      #define QUATERNION_DERIVATIVE
47        #ifdef QUATERNION_DERIVATIVE
48                btQuaternion predictedOrn = curTrans.getRotation();
49                predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
50                predictedOrn.normalize();
51        #else
52                //Exponential map
53                //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
54
55                btVector3 axis;
56                btScalar        fAngle = angvel.length(); 
57                //limit the angular motion
58                if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
59                {
60                        fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
61                }
62
63                if ( fAngle < btScalar(0.001) )
64                {
65                        // use Taylor's expansions of sync function
66                        axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
67                }
68                else
69                {
70                        // sync(fAngle) = sin(c*fAngle)/t
71                        axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
72                }
73                btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
74                btQuaternion orn0 = curTrans.getRotation();
75
76                btQuaternion predictedOrn = dorn * orn0;
77                predictedOrn.normalize();
78        #endif
79                predictedTransform.setRotation(predictedOrn);
80        }
81
82        static void     calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
83        {
84                linVel = (pos1 - pos0) / timeStep;
85                btVector3 axis;
86                btScalar  angle;
87                if (orn0 != orn1)
88                {
89                        calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
90                        angVel = axis * angle / timeStep;
91                } else
92                {
93                        angVel.setValue(0,0,0);
94                }
95        }
96
97        static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
98        {
99                btQuaternion orn1 = orn0.nearest(orn1a);
100                btQuaternion dorn = orn1 * orn0.inverse();
101                angle = dorn.getAngle();
102                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
103                axis[3] = btScalar(0.);
104                //check for axis length
105                btScalar len = axis.length2();
106                if (len < SIMD_EPSILON*SIMD_EPSILON)
107                        axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
108                else
109                        axis /= btSqrt(len);
110        }
111
112        static void     calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
113        {
114                linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
115                btVector3 axis;
116                btScalar  angle;
117                calculateDiffAxisAngle(transform0,transform1,axis,angle);
118                angVel = axis * angle / timeStep;
119        }
120
121        static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
122        {
123                btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
124                btQuaternion dorn;
125                dmat.getRotation(dorn);
126
127                ///floating point inaccuracy can lead to w component > 1..., which breaks
128                dorn.normalize();
129               
130                angle = dorn.getAngle();
131                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
132                axis[3] = btScalar(0.);
133                //check for axis length
134                btScalar len = axis.length2();
135                if (len < SIMD_EPSILON*SIMD_EPSILON)
136                        axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
137                else
138                        axis /= btSqrt(len);
139        }
140
141};
142
143
144///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
145///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
146class   btConvexSeparatingDistanceUtil
147{
148        btQuaternion    m_ornA;
149        btQuaternion    m_ornB;
150        btVector3       m_posA;
151        btVector3       m_posB;
152       
153        btVector3       m_separatingNormal;
154
155        btScalar        m_boundingRadiusA;
156        btScalar        m_boundingRadiusB;
157        btScalar        m_separatingDistance;
158
159public:
160
161        btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar        boundingRadiusB)
162                :m_boundingRadiusA(boundingRadiusA),
163                m_boundingRadiusB(boundingRadiusB),
164                m_separatingDistance(0.f)
165        {
166        }
167
168        btScalar        getConservativeSeparatingDistance()
169        {
170                return m_separatingDistance;
171        }
172
173        void    updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
174        {
175                const btVector3& toPosA = transA.getOrigin();
176                const btVector3& toPosB = transB.getOrigin();
177                btQuaternion toOrnA = transA.getRotation();
178                btQuaternion toOrnB = transB.getRotation();
179
180                if (m_separatingDistance>0.f)
181                {
182                       
183
184                        btVector3 linVelA,angVelA,linVelB,angVelB;
185                        btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
186                        btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
187                        btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
188                        btVector3 relLinVel = (linVelB-linVelA);
189                        btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
190                        if (relLinVelocLength<0.f)
191                        {
192                                relLinVelocLength = 0.f;
193                        }
194       
195                        btScalar        projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
196                        m_separatingDistance -= projectedMotion;
197                }
198       
199                m_posA = toPosA;
200                m_posB = toPosB;
201                m_ornA = toOrnA;
202                m_ornB = toOrnB;
203        }
204
205        void    initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
206        {
207                m_separatingDistance = separatingDistance;
208
209                if (m_separatingDistance>0.f)
210                {
211                        m_separatingNormal = separatingVector;
212                       
213                        const btVector3& toPosA = transA.getOrigin();
214                        const btVector3& toPosB = transB.getOrigin();
215                        btQuaternion toOrnA = transA.getRotation();
216                        btQuaternion toOrnB = transB.getRotation();
217                        m_posA = toPosA;
218                        m_posB = toPosB;
219                        m_ornA = toOrnA;
220                        m_ornB = toOrnB;
221                }
222        }
223
224};
225
226
227#endif //BT_TRANSFORM_UTIL_H
228
Note: See TracBrowser for help on using the repository browser.