1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
|
/*
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erwin Coumans makes no representations about the suitability
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#ifndef SIMD_TRANSFORM_UTIL_H
#define SIMD_TRANSFORM_UTIL_H
#include "SimdTransform.h"
#define ANGULAR_MOTION_TRESHOLD 0.5f*SIMD_HALF_PI
/// Utils related to temporal transforms
class SimdTransformUtil
{
public:
static void IntegrateTransform(const SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep,SimdTransform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
SimdQuaternion orn = curTrans.getRotation();
orn += (angvel * orn) * (timeStep * 0.5f);
orn.normalize();
#else
//exponential map
SimdVector3 axis;
SimdScalar fAngle = angvel.length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_TRESHOLD)
{
fAngle = ANGULAR_MOTION_TRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( SimdSin(0.5f*fAngle*timeStep)/fAngle );
}
SimdQuaternion dorn (axis.x(),axis.y(),axis.z(),SimdCos( fAngle*timeStep*0.5f ));
SimdQuaternion orn0 = curTrans.getRotation();
SimdQuaternion predictedOrn = dorn * orn0;
#endif
predictedTransform.setRotation(predictedOrn);
}
static void CalculateVelocity(const SimdTransform& transform0,const SimdTransform& transform1,SimdScalar timeStep,SimdVector3& linVel,SimdVector3& angVel)
{
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
#ifdef USE_QUATERNION_DIFF
SimdQuaternion orn0 = transform0.getRotation();
SimdQuaternion orn1a = transform1.getRotation();
SimdQuaternion orn1 = orn0.farthest(orn1a);
SimdQuaternion dorn = orn1 * orn0.inverse();
#else
SimdMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
SimdQuaternion dorn;
dmat.getRotation(dorn);
#endif//USE_QUATERNION_DIFF
SimdVector3 axis;
SimdScalar angle;
angle = dorn.getAngle();
axis = SimdVector3(dorn.x(),dorn.y(),dorn.z());
axis[3] = 0.f;
//check for axis length
SimdScalar len = axis.length2();
if (len < 0.001f)
axis = SimdVector3(1.f,0.f,0.f);
else
axis /= SimdSqrt(len);
angVel = axis * angle / timeStep;
}
};
#endif //SIMD_TRANSFORM_UTIL_H
|
