55 lines
1.4 KiB
TypeScript
55 lines
1.4 KiB
TypeScript
|
/**
|
||
|
|
* @author Martin Karkowski
|
||
|
|
* @email m.karkowski@zema.de
|
||
|
|
* @create date 2018-06-20 03:09:56
|
||
|
|
* @modify date 2018-06-24 05:58:55
|
||
|
|
* @desc [description]
|
||
|
|
*/
|
||
|
|
|
||
|
|
import * as geometry from '../src/geometric-functions';
|
||
|
|
import * as math from 'mathjs';
|
||
|
|
|
||
|
|
console.log(geometry.QuatToRPY(geometry.rotMatrixToQuat(geometry.quatToRotMatrix(geometry.RPYtoQuat({ roll: 1, pitch: 0, yaw: 0 })))));
|
||
|
|
|
||
|
|
const _quat = {
|
||
|
|
imgX: 0,
|
||
|
|
imgY: 0,
|
||
|
|
imgZ: 0,
|
||
|
|
real: 1
|
||
|
|
};
|
||
|
|
|
||
|
|
const _rpy = {
|
||
|
|
roll: math.pi*0.5,
|
||
|
|
pitch: 0,
|
||
|
|
yaw: 0
|
||
|
|
}
|
||
|
|
|
||
|
|
const _originTrans = {
|
||
|
|
x: 0,
|
||
|
|
y: 0,
|
||
|
|
z: 0
|
||
|
|
};
|
||
|
|
|
||
|
|
const _transTo = {
|
||
|
|
x: 1,
|
||
|
|
y: 2,
|
||
|
|
z: 3
|
||
|
|
};
|
||
|
|
|
||
|
|
console.log('TEST QUAT => MAT => QUAT', geometry.rotMatrixToQuat(geometry.quatToRotMatrix({imgX:0,imgY:-1,imgZ:0,real:Math.PI/2})));
|
||
|
|
|
||
|
|
const _origin = geometry.homogenousCoordinates(_originTrans, _quat);
|
||
|
|
const _target = geometry.homogenousCoordinates(_transTo, geometry.RPYtoQuat(_rpy));
|
||
|
|
const _transformation = geometry.createTransformation(_origin, _target);
|
||
|
|
|
||
|
|
console.log(
|
||
|
|
'From:\n', _origin.matrix.toArray(),
|
||
|
|
'To:\n', _target.matrix.toArray(),
|
||
|
|
'\nTransformation:\n', _transformation.matrix.toArray(),
|
||
|
|
'\nDestionation:\n', math.multiply(_transformation.matrix, _origin.matrix).toArray());
|
||
|
|
|
||
|
|
console.log(geometry.extractTranslation(_transformation));
|
||
|
|
|
||
|
|
console.log(geometry.extractQuaternion(_transformation));
|
||
|
|
|
||
|
|
console.log(geometry.transformPoint(_transformation, {x:1,y:1,z:1}));
|