void TransformationMatrix::recompose(const DecomposedType& decomp)
{
makeIdentity();
// first apply perspective
m_matrix[0][3] = (float) decomp.perspectiveX;
m_matrix[1][3] = (float) decomp.perspectiveY;
m_matrix[2][3] = (float) decomp.perspectiveZ;
m_matrix[3][3] = (float) decomp.perspectiveW;
// now translate
translate3d((float) decomp.translateX, (float) decomp.translateY, (float) decomp.translateZ);
// apply rotation
double xx = decomp.quaternionX * decomp.quaternionX;
double xy = decomp.quaternionX * decomp.quaternionY;
double xz = decomp.quaternionX * decomp.quaternionZ;
double xw = decomp.quaternionX * decomp.quaternionW;
double yy = decomp.quaternionY * decomp.quaternionY;
double yz = decomp.quaternionY * decomp.quaternionZ;
double yw = decomp.quaternionY * decomp.quaternionW;
double zz = decomp.quaternionZ * decomp.quaternionZ;
double zw = decomp.quaternionZ * decomp.quaternionW;
// Construct a composite rotation matrix from the quaternion values
TransformationMatrix rotationMatrix(1 - 2 * (yy + zz), 2 * (xy - zw), 2 * (xz + yw), 0,
2 * (xy + zw), 1 - 2 * (xx + zz), 2 * (yz - xw), 0,
2 * (xz - yw), 2 * (yz + xw), 1 - 2 * (xx + yy), 0,
0, 0, 0, 1);
multLeft(rotationMatrix);
// now apply skew
if (decomp.skewYZ) {
TransformationMatrix tmp;
tmp.setM32((float) decomp.skewYZ);
multLeft(tmp);
}
if (decomp.skewXZ) {
TransformationMatrix tmp;
tmp.setM31((float) decomp.skewXZ);
multLeft(tmp);
}
if (decomp.skewXY) {
TransformationMatrix tmp;
tmp.setM21((float) decomp.skewXY);
multLeft(tmp);
}
// finally, apply scale
scale3d((float) decomp.scaleX, (float) decomp.scaleY, (float) decomp.scaleZ);
}
TransformationMatrix& TransformationMatrix::applyPerspective(double p)
{
TransformationMatrix mat;
if (p != 0)
mat.m_matrix[2][3] = -1/p;
multLeft(mat);
return *this;
}
TransformationMatrix& TransformationMatrix::scaleNonUniform(double sx, double sy)
{
TransformationMatrix mat;
mat.m_matrix[0][0] = sx;
mat.m_matrix[1][1] = sy;
multLeft(mat);
return *this;
}
TransformationMatrix& TransformationMatrix::scale3d(double sx, double sy, double sz)
{
TransformationMatrix mat;
mat.m_matrix[0][0] = sx;
mat.m_matrix[1][1] = sy;
mat.m_matrix[2][2] = sz;
multLeft(mat);
return *this;
}
AffineTransform& AffineTransform::rotate(double a)
{
// angle is in degree. Switch to radian
a = deg2rad(a);
double cosAngle = cos(a);
double sinAngle = sin(a);
AffineTransform rot(cosAngle, sinAngle, -sinAngle, cosAngle, 0, 0);
multLeft(rot);
return *this;
}
TransformationMatrix& TransformationMatrix::skew(double sx, double sy)
{
// angles are in degrees. Switch to radians
sx = deg2rad(sx);
sy = deg2rad(sy);
TransformationMatrix mat;
mat.m_matrix[0][1] = tan(sy); // note that the y shear goes in the first row
mat.m_matrix[1][0] = tan(sx); // and the x shear in the second row
multLeft(mat);
return *this;
}
void
SbMatrix::setTransform(const SbVec3f &translation,
const SbRotation &rotation,
const SbVec3f &scaleFactor,
const SbRotation &scaleOrientation,
const SbVec3f ¢er)
{
#define TRANSLATE(vec) m.setTranslate(vec), multLeft(m)
#define ROTATE(rot) rot.getValue(m), multLeft(m)
SbMatrix m;
makeIdentity();
if (translation != SbVec3f(0,0,0))
TRANSLATE(translation);
if (center != SbVec3f(0,0,0))
TRANSLATE(center);
if (rotation != SbRotation(0,0,0,1))
ROTATE(rotation);
if (scaleFactor != SbVec3f(1,1,1)) {
SbRotation so = scaleOrientation;
if (so != SbRotation(0,0,0,1))
ROTATE(so);
m.setScale(scaleFactor);
multLeft(m);
if (so != SbRotation(0,0,0,1)) {
so.invert();
ROTATE(so);
}
}
if (center != SbVec3f(0,0,0))
TRANSLATE(-center);
#undef TRANSLATE
#undef ROTATE
}
TransformationMatrix& TransformationMatrix::rotate3d(double rx, double ry, double rz)
{
// angles are in degrees. Switch to radians
rx = deg2rad(rx);
ry = deg2rad(ry);
rz = deg2rad(rz);
TransformationMatrix mat;
rz /= 2.0f;
double sinA = sin(rz);
double cosA = cos(rz);
double sinA2 = sinA * sinA;
mat.m_matrix[0][0] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][1] = 2.0f * sinA * cosA;
mat.m_matrix[0][2] = 0.0f;
mat.m_matrix[1][0] = -2.0f * sinA * cosA;
mat.m_matrix[1][1] = 1.0f - 2.0f * sinA2;
mat.m_matrix[1][2] = 0.0f;
mat.m_matrix[2][0] = 0.0f;
mat.m_matrix[2][1] = 0.0f;
mat.m_matrix[2][2] = 1.0f;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
TransformationMatrix rmat(mat);
ry /= 2.0f;
sinA = sin(ry);
cosA = cos(ry);
sinA2 = sinA * sinA;
mat.m_matrix[0][0] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][1] = 0.0f;
mat.m_matrix[0][2] = -2.0f * sinA * cosA;
mat.m_matrix[1][0] = 0.0f;
mat.m_matrix[1][1] = 1.0f;
mat.m_matrix[1][2] = 0.0f;
mat.m_matrix[2][0] = 2.0f * sinA * cosA;
mat.m_matrix[2][1] = 0.0f;
mat.m_matrix[2][2] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
rmat.multLeft(mat);
rx /= 2.0f;
sinA = sin(rx);
cosA = cos(rx);
sinA2 = sinA * sinA;
mat.m_matrix[0][0] = 1.0f;
mat.m_matrix[0][1] = 0.0f;
mat.m_matrix[0][2] = 0.0f;
mat.m_matrix[1][0] = 0.0f;
mat.m_matrix[1][1] = 1.0f - 2.0f * sinA2;
mat.m_matrix[1][2] = 2.0f * sinA * cosA;
mat.m_matrix[2][0] = 0.0f;
mat.m_matrix[2][1] = -2.0f * sinA * cosA;
mat.m_matrix[2][2] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
rmat.multLeft(mat);
multLeft(rmat);
return *this;
}
TransformationMatrix& TransformationMatrix::rotate3d(double x, double y, double z, double angle)
{
// angles are in degrees. Switch to radians
angle = deg2rad(angle);
angle /= 2.0f;
double sinA = sin(angle);
double cosA = cos(angle);
double sinA2 = sinA * sinA;
// normalize
double length = sqrt(x * x + y * y + z * z);
if (length == 0) {
// bad vector, just use something reasonable
x = 0;
y = 0;
z = 1;
} else if (length != 1) {
x /= length;
y /= length;
z /= length;
}
TransformationMatrix mat;
// optimize case where axis is along major axis
if (x == 1.0f && y == 0.0f && z == 0.0f) {
mat.m_matrix[0][0] = 1.0f;
mat.m_matrix[0][1] = 0.0f;
mat.m_matrix[0][2] = 0.0f;
mat.m_matrix[1][0] = 0.0f;
mat.m_matrix[1][1] = 1.0f - 2.0f * sinA2;
mat.m_matrix[1][2] = 2.0f * sinA * cosA;
mat.m_matrix[2][0] = 0.0f;
mat.m_matrix[2][1] = -2.0f * sinA * cosA;
mat.m_matrix[2][2] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
} else if (x == 0.0f && y == 1.0f && z == 0.0f) {
mat.m_matrix[0][0] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][1] = 0.0f;
mat.m_matrix[0][2] = -2.0f * sinA * cosA;
mat.m_matrix[1][0] = 0.0f;
mat.m_matrix[1][1] = 1.0f;
mat.m_matrix[1][2] = 0.0f;
mat.m_matrix[2][0] = 2.0f * sinA * cosA;
mat.m_matrix[2][1] = 0.0f;
mat.m_matrix[2][2] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
} else if (x == 0.0f && y == 0.0f && z == 1.0f) {
mat.m_matrix[0][0] = 1.0f - 2.0f * sinA2;
mat.m_matrix[0][1] = 2.0f * sinA * cosA;
mat.m_matrix[0][2] = 0.0f;
mat.m_matrix[1][0] = -2.0f * sinA * cosA;
mat.m_matrix[1][1] = 1.0f - 2.0f * sinA2;
mat.m_matrix[1][2] = 0.0f;
mat.m_matrix[2][0] = 0.0f;
mat.m_matrix[2][1] = 0.0f;
mat.m_matrix[2][2] = 1.0f;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
} else {
double x2 = x*x;
double y2 = y*y;
double z2 = z*z;
mat.m_matrix[0][0] = 1.0f - 2.0f * (y2 + z2) * sinA2;
mat.m_matrix[0][1] = 2.0f * (x * y * sinA2 + z * sinA * cosA);
mat.m_matrix[0][2] = 2.0f * (x * z * sinA2 - y * sinA * cosA);
mat.m_matrix[1][0] = 2.0f * (y * x * sinA2 - z * sinA * cosA);
mat.m_matrix[1][1] = 1.0f - 2.0f * (z2 + x2) * sinA2;
mat.m_matrix[1][2] = 2.0f * (y * z * sinA2 + x * sinA * cosA);
mat.m_matrix[2][0] = 2.0f * (z * x * sinA2 + y * sinA * cosA);
mat.m_matrix[2][1] = 2.0f * (z * y * sinA2 - x * sinA * cosA);
mat.m_matrix[2][2] = 1.0f - 2.0f * (x2 + y2) * sinA2;
mat.m_matrix[0][3] = mat.m_matrix[1][3] = mat.m_matrix[2][3] = 0.0f;
mat.m_matrix[3][0] = mat.m_matrix[3][1] = mat.m_matrix[3][2] = 0.0f;
mat.m_matrix[3][3] = 1.0f;
}
multLeft(mat);
return *this;
}
