void OnlineRotHec::addMeasurement( const Math::Quaternion& q, const Math::Quaternion& r )
{
// make sure the signs of both w's are equal
const double nq = q.w() < 0 ? -1 : 1;
const double nr = r.w() < 0 ? -1 : 1;
Math::ErrorVector< double, 3 > kalmanMeasurement;
kalmanMeasurement.value( 0 ) = r.x() * nr - q.x() * nq;
kalmanMeasurement.value( 1 ) = r.y() * nr - q.y() * nq;
kalmanMeasurement.value( 2 ) = r.z() * nr - q.z() * nq;
kalmanMeasurement.covariance = Math::Matrix< double, 3, 3 >::identity();
// do the filter update
Math::Matrix< double, 3, 3 > h;
skewMatrix( h, Math::Vector< double, 3 >( q.x() * nq + r.x() * nr, q.y() * nq + r.y() * nr, q.z() * nq + r.z() * nr ) );
Tracking::kalmanMeasurementUpdate< 3, 3 >( m_state, Math::Function::LinearFunction< 3, 3, double >( h ),
kalmanMeasurement, 0, m_state.value.size() );
}
const Mat4& Node::getNodeToParentTransform() const
{
if (_transformDirty)
{
// Translate values
float x = _position.x;
float y = _position.y;
float z = _positionZ;
if (_ignoreAnchorPointForPosition)
{
x += _anchorPointInPoints.x;
y += _anchorPointInPoints.y;
}
// Rotation values
// Change rotation code to handle X and Y
// If we skew with the exact same value for both x and y then we're simply just rotating
float cx = 1, sx = 0, cy = 1, sy = 0;
if (_rotationZ_X || _rotationZ_Y)
{
float radiansX = -CC_DEGREES_TO_RADIANS(_rotationZ_X);
float radiansY = -CC_DEGREES_TO_RADIANS(_rotationZ_Y);
cx = cosf(radiansX);
sx = sinf(radiansX);
cy = cosf(radiansY);
sy = sinf(radiansY);
}
bool needsSkewMatrix = ( _skewX || _skewY );
// optimization:
// inline anchor point calculation if skew is not needed
// Adjusted transform calculation for rotational skew
if (! needsSkewMatrix && !_anchorPointInPoints.equals(Vec2::ZERO))
{
x += cy * -_anchorPointInPoints.x * _scaleX + -sx * -_anchorPointInPoints.y * _scaleY;
y += sy * -_anchorPointInPoints.x * _scaleX + cx * -_anchorPointInPoints.y * _scaleY;
}
// Build Transform Matrix
// Adjusted transform calculation for rotational skew
float mat[] = {
cy * _scaleX, sy * _scaleX, 0, 0,
-sx * _scaleY, cx * _scaleY, 0, 0,
0, 0, _scaleZ, 0,
x, y, z, 1 };
_transform.set(mat);
// XXX
// FIX ME: Expensive operation.
// FIX ME: It should be done together with the rotationZ
if(_rotationY) {
Mat4 rotY;
Mat4::createRotationY(CC_DEGREES_TO_RADIANS(_rotationY), &rotY);
_transform = _transform * rotY;
}
if(_rotationX) {
Mat4 rotX;
Mat4::createRotationX(CC_DEGREES_TO_RADIANS(_rotationX), &rotX);
_transform = _transform * rotX;
}
// XXX: Try to inline skew
// If skew is needed, apply skew and then anchor point
if (needsSkewMatrix)
{
Mat4 skewMatrix(1, (float)tanf(CC_DEGREES_TO_RADIANS(_skewY)), 0, 0,
(float)tanf(CC_DEGREES_TO_RADIANS(_skewX)), 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
_transform = _transform * skewMatrix;
// adjust anchor point
if (!_anchorPointInPoints.equals(Vec2::ZERO))
{
// XXX: Argh, Mat4 needs a "translate" method.
// XXX: Although this is faster than multiplying a vec4 * mat4
_transform.m[12] += _transform.m[0] * -_anchorPointInPoints.x + _transform.m[4] * -_anchorPointInPoints.y;
_transform.m[13] += _transform.m[1] * -_anchorPointInPoints.x + _transform.m[5] * -_anchorPointInPoints.y;
}
}
if (_useAdditionalTransform)
{
_transform = _transform * _additionalTransform;
}
_transformDirty = false;
}
return _transform;
}
const Mat4& Node::getNodeToParentTransform() const
{
if (_transformDirty)
{
// Translate values
float x = _position.x;
float y = _position.y;
float z = _positionZ;
if (_ignoreAnchorPointForPosition)
{
x += _anchorPointInPoints.x;
y += _anchorPointInPoints.y;
}
bool needsSkewMatrix = ( _skewX || _skewY );
Vec2 anchorPoint(_anchorPointInPoints.x * _scaleX, _anchorPointInPoints.y * _scaleY);
// calculate real position
if (! needsSkewMatrix && !_anchorPointInPoints.isZero())
{
x += -anchorPoint.x;
y += -anchorPoint.y;
}
// Build Transform Matrix = translation * rotation * scale
Mat4 translation;
//move to anchor point first, then rotate
Mat4::createTranslation(x + anchorPoint.x, y + anchorPoint.y, z, &translation);
Mat4::createRotation(_rotationQuat, &_transform);
if (_rotationZ_X != _rotationZ_Y)
{
// Rotation values
// Change rotation code to handle X and Y
// If we skew with the exact same value for both x and y then we're simply just rotating
float radiansX = -CC_DEGREES_TO_RADIANS(_rotationZ_X);
float radiansY = -CC_DEGREES_TO_RADIANS(_rotationZ_Y);
float cx = cosf(radiansX);
float sx = sinf(radiansX);
float cy = cosf(radiansY);
float sy = sinf(radiansY);
float m0 = _transform.m[0], m1 = _transform.m[1], m4 = _transform.m[4], m5 = _transform.m[5], m8 = _transform.m[8], m9 = _transform.m[9];
_transform.m[0] = cy * m0 - sx * m1, _transform.m[4] = cy * m4 - sx * m5, _transform.m[8] = cy * m8 - sx * m9;
_transform.m[1] = sy * m0 + cx * m1, _transform.m[5] = sy * m4 + cx * m5, _transform.m[9] = sy * m8 + cx * m9;
}
_transform = translation * _transform;
//move by (-anchorPoint.x, -anchorPoint.y, 0) after rotation
_transform.translate(-anchorPoint.x, -anchorPoint.y, 0);
if (_scaleX != 1.f)
{
_transform.m[0] *= _scaleX, _transform.m[1] *= _scaleX, _transform.m[2] *= _scaleX;
}
if (_scaleY != 1.f)
{
_transform.m[4] *= _scaleY, _transform.m[5] *= _scaleY, _transform.m[6] *= _scaleY;
}
if (_scaleZ != 1.f)
{
_transform.m[8] *= _scaleZ, _transform.m[9] *= _scaleZ, _transform.m[10] *= _scaleZ;
}
// FIXME:: Try to inline skew
// If skew is needed, apply skew and then anchor point
if (needsSkewMatrix)
{
float skewMatArray[16] =
{
1, (float)tanf(CC_DEGREES_TO_RADIANS(_skewY)), 0, 0,
(float)tanf(CC_DEGREES_TO_RADIANS(_skewX)), 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
};
Mat4 skewMatrix(skewMatArray);
_transform = _transform * skewMatrix;
// adjust anchor point
if (!_anchorPointInPoints.isZero())
{
// FIXME:: Argh, Mat4 needs a "translate" method.
// FIXME:: Although this is faster than multiplying a vec4 * mat4
_transform.m[12] += _transform.m[0] * -_anchorPointInPoints.x + _transform.m[4] * -_anchorPointInPoints.y;
_transform.m[13] += _transform.m[1] * -_anchorPointInPoints.x + _transform.m[5] * -_anchorPointInPoints.y;
}
}
if (_useAdditionalTransform)
{
_transform = _transform * _additionalTransform;
}
_transformDirty = false;
}
return _transform;
}
