00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef __DWARFUTIL_H_
00019 #define __DWARFUTIL_H_
00020
00021 #include <DWARF/DwarfCommon.h>
00022
00023
00024 namespace DWARF {
00025 namespace Util {
00026
00027
00028
00029
00030
00031
00032 Time currentTime();
00033
00034
00035
00036
00037
00038
00039 Time getTimeToNow(Time &compareTime);
00040
00041
00042
00043
00044
00045 const unsigned char QX = 0;
00046
00047 const unsigned char QY = 1;
00048
00049 const unsigned char QZ = 2;
00050
00051 const unsigned char QW = 3;
00052
00053 inline double q_x( double *q) { return q[QX]; }
00054
00055 inline double q_y( double *q) { return q[QY]; }
00056
00057 inline double q_z( double *q) { return q[QZ]; }
00058
00059 inline double q_w( double *q) { return q[QW]; }
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079 double *quaternionToMatrix( const double *quat, double *matrix);
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100 double *matrixToQuaternion( const double *matrix, double *quat );
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 double* axisAngleToQuaternion( double* axisAngle, double* quat );
00111 inline double* vrmlToQuaternion(double* vrml, double* q) {
00112 return axisAngleToQuaternion(vrml, q);
00113 }
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123 double* quaternionToAxisAngle( double* quat, double* axisAngle );
00124 inline double* quaternionToVrml(double* q, double* vrml) {
00125 return quaternionToAxisAngle(q, vrml);
00126 }
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138 double* invertQuaternion( double *quat );
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 double* conjugateQuaternion( double *quat );
00149
00150
00151
00152
00153
00154
00155
00156 double* normalizeQuaternion( double *quat );
00157
00158
00159
00160
00161
00162
00163
00164
00165 double* normalizeVector( double *vec, int length );
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176 double* multiplyQuaternion( double *quat, const double *quat1, const double *quat2 );
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188 double* rotateQuaternion( double *rotPoint, const double *quat, const double *point );
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202 void transformCoordinateFrame( const double *transRot, const double *transPos,
00203 double *rot, double *pos);
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 double* rotateMatrix( double *rotPoint, const double *matrix, const double *point );
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227 double* invertMatrix( double *matrix, int col, int row );
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239 double* crossProduct3D( double *p, double *a, double *b );
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249 double scalarProduct3D( double *a, double *b );
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259 double angleBetweenVectors3D( double *a, double *b );
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275 void poseRightInverse( double* newQuat, double* newTrans,
00276 const double* oldQuat, const double* oldTrans );
00277 inline void invertQuaternionPose( double* newQuat, double* newTrans,
00278 const double* oldQuat, const double* oldTrans ) {
00279 return poseRightInverse(newQuat, newTrans, oldQuat, oldTrans);
00280 }
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296 void poseLeftInverse( double* newQuat, double* newTrans,
00297 const double* oldQuat, const double* oldTrans );
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322 double* colMatrixToEuler( double* colMatrix, double* angles );
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334 double* eulerToQuaternion( double* angles, double* quat );
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345 double* quaternionSlerp( double* quat, const double* quat1, const double* quat2, double h );
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360 double* quaternionNegateIfCloser( double* quat, const double* otherquat );
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370 double* negateQuaternion( double *quat );
00371 };
00372 };
00373
00374
00375
00376 #endif