trans2D_t *invert_transform(trans2D_t *T) {
double d = det(T);
double cofactor;
int i, j;
int one[3] = { 1, 0, 0 };
int two[3] = { 2, 2, 1 };
trans2D_t *Tinv = new_identity_transform(), *I;
/* Check if the transform is invertible */
if (d == 0.0) {
printf("Singular transformation detected!\n");
return NULL;
}
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
int parity = (i+j) % 2;
cofactor = (parity ? -1.0 : 1.0) *
(T->T[one[i]][one[j]] * T->T[two[i]][two[j]] -
T->T[one[i]][two[j]] * T->T[two[i]][one[j]]);
Tinv->T[j][i] = cofactor / d;
}
}
I = transform_product(T, Tinv);
printf("Tinv:\n");
print_transform(Tinv);
printf("I:\n");
print_transform(I);
transform_free(I);
return Tinv;
}
static void test_transforms()
{
double PI = carmen_degrees_to_radians(180);
tf::Transform t1;
t1.setOrigin(tf::Vector3(1.0, 0.0, 0.0));
t1.setRotation(tf::Quaternion(PI/2, 0.0, 0.0));
tf::Transform t2;
t2.setOrigin(tf::Vector3(1.0, 0.0, 0.0));
t2.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tf::Transform t3 = t1*t2;
print_transform(t1);
print_transform(t2);
print_transform(t3);
}
