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); }