Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/bullet/LinearMath/btTransformUtil.h @ 5726

Last change on this file since 5726 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

  • Property svn:eol-style set to native
File size: 7.8 KB
RevLine 
[1963]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 SIMD_TRANSFORM_UTIL_H
17#define SIMD_TRANSFORM_UTIL_H
18
19#include "btTransform.h"
20#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
21
22
23
24#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
25
26#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
27
28SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
29{
30        return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
31      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
32      supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 
33}
34
35
36SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
37{
38  if (btFabs(n.z()) > SIMDSQRT12) {
39    // choose p in y-z plane
40    btScalar a = n[1]*n[1] + n[2]*n[2];
41    btScalar k = btRecipSqrt (a);
42    p.setValue(0,-n[2]*k,n[1]*k);
43    // set q = n x p
44    q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
45  }
46  else {
47    // choose p in x-y plane
48    btScalar a = n.x()*n.x() + n.y()*n.y();
49    btScalar k = btRecipSqrt (a);
50    p.setValue(-n.y()*k,n.x()*k,0);
51    // set q = n x p
52    q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
53  }
54}
55
56
57
58/// Utils related to temporal transforms
59class btTransformUtil
60{
61
62public:
63
64        static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
65        {
66                predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
67//      #define QUATERNION_DERIVATIVE
68        #ifdef QUATERNION_DERIVATIVE
69                btQuaternion predictedOrn = curTrans.getRotation();
70                predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
71                predictedOrn.normalize();
72        #else
73                //Exponential map
74                //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
75
76                btVector3 axis;
77                btScalar        fAngle = angvel.length(); 
78                //limit the angular motion
79                if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
80                {
81                        fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
82                }
83
84                if ( fAngle < btScalar(0.001) )
85                {
86                        // use Taylor's expansions of sync function
87                        axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
88                }
89                else
90                {
91                        // sync(fAngle) = sin(c*fAngle)/t
92                        axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
93                }
94                btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
95                btQuaternion orn0 = curTrans.getRotation();
96
97                btQuaternion predictedOrn = dorn * orn0;
98                predictedOrn.normalize();
99        #endif
100                predictedTransform.setRotation(predictedOrn);
101        }
102
[2430]103        static void     calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
104        {
105                linVel = (pos1 - pos0) / timeStep;
106                btVector3 axis;
107                btScalar  angle;
108                if (orn0 != orn1)
109                {
110                        calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
111                        angVel = axis * angle / timeStep;
112                } else
113                {
114                        angVel.setValue(0,0,0);
115                }
116        }
117
118        static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
119        {
120                btQuaternion orn1 = orn0.farthest(orn1a);
121                btQuaternion dorn = orn1 * orn0.inverse();
122                ///floating point inaccuracy can lead to w component > 1..., which breaks
123                dorn.normalize();
124                angle = dorn.getAngle();
125                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
126                axis[3] = btScalar(0.);
127                //check for axis length
128                btScalar len = axis.length2();
129                if (len < SIMD_EPSILON*SIMD_EPSILON)
130                        axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
131                else
132                        axis /= btSqrt(len);
133        }
134
[1963]135        static void     calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
136        {
137                linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
138                btVector3 axis;
139                btScalar  angle;
140                calculateDiffAxisAngle(transform0,transform1,axis,angle);
141                angVel = axis * angle / timeStep;
142        }
143
144        static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
145        {
146                btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
147                btQuaternion dorn;
148                dmat.getRotation(dorn);
[2430]149
[1963]150                ///floating point inaccuracy can lead to w component > 1..., which breaks
151                dorn.normalize();
152               
153                angle = dorn.getAngle();
154                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
155                axis[3] = btScalar(0.);
156                //check for axis length
157                btScalar len = axis.length2();
158                if (len < SIMD_EPSILON*SIMD_EPSILON)
159                        axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
160                else
161                        axis /= btSqrt(len);
162        }
163
164};
165
[2430]166
167///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
168///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
169class   btConvexSeparatingDistanceUtil
170{
171        btQuaternion    m_ornA;
172        btQuaternion    m_ornB;
173        btVector3       m_posA;
174        btVector3       m_posB;
175       
176        btVector3       m_separatingNormal;
177
178        btScalar        m_boundingRadiusA;
179        btScalar        m_boundingRadiusB;
180        btScalar        m_separatingDistance;
181
182public:
183
184        btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar        boundingRadiusB)
185                :m_boundingRadiusA(boundingRadiusA),
186                m_boundingRadiusB(boundingRadiusB),
187                m_separatingDistance(0.f)
188        {
189        }
190
191        btScalar        getConservativeSeparatingDistance()
192        {
193                return m_separatingDistance;
194        }
195
196        void    updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
197        {
198                const btVector3& toPosA = transA.getOrigin();
199                const btVector3& toPosB = transB.getOrigin();
200                btQuaternion toOrnA = transA.getRotation();
201                btQuaternion toOrnB = transB.getRotation();
202
203                if (m_separatingDistance>0.f)
204                {
205                       
206
207                        btVector3 linVelA,angVelA,linVelB,angVelB;
208                        btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
209                        btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
210                        btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
211                        btVector3 relLinVel = (linVelB-linVelA);
212                        btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
213                        if (relLinVelocLength<0.f)
214                        {
215                                relLinVelocLength = 0.f;
216                        }
217       
218                        btScalar        projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
219                        m_separatingDistance -= projectedMotion;
220                }
221       
222                m_posA = toPosA;
223                m_posB = toPosB;
224                m_ornA = toOrnA;
225                m_ornB = toOrnB;
226        }
227
228        void    initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
229        {
230                m_separatingNormal = separatingVector;
231                m_separatingDistance = separatingDistance;
232               
233                const btVector3& toPosA = transA.getOrigin();
234                const btVector3& toPosB = transB.getOrigin();
235                btQuaternion toOrnA = transA.getRotation();
236                btQuaternion toOrnB = transB.getRotation();
237                m_posA = toPosA;
238                m_posB = toPosB;
239                m_ornA = toOrnA;
240                m_ornB = toOrnB;
241        }
242
243};
244
245
[1963]246#endif //SIMD_TRANSFORM_UTIL_H
247
Note: See TracBrowser for help on using the repository browser.