void LWQUAT_applyMatrix(LWFMatrix4 M, LWFQuat Q, LWFQuat qu) /* Uses Q as a regular old 3-vector */ { QSET(qu, 0.0f); qu[X] = M[0][0]*Q[X] + M[0][1]*Q[Y] + M[0][2]*Q[Z] + M[0][3]*Q[W] ; qu[Y] = M[1][0]*Q[X] + M[1][1]*Q[Y] + M[1][2]*Q[Z] + M[1][3]*Q[W] ; qu[Z] = M[2][0]*Q[X] + M[2][1]*Q[Y] + M[2][2]*Q[Z] + M[2][3]*Q[W] ; qu[W] = M[3][0]*Q[X] + M[3][1]*Q[Y] + M[3][2]*Q[Z] + M[3][3]*Q[W] ; }
/* * Returns - * -2 unknown keyword * -1 error in processing keyword * 0 OK */ int multi_words( char *words[], int nwords ) { if ( strcmp( words[0], "rot" ) == 0 ) { mat_t mat; /* Expects rotations rx, ry, rz, in degrees */ if ( nwords < 4 ) return(-1); MAT_IDN( mat ); bn_mat_angles( mat, atof( words[1] ), atof( words[2] ), atof( words[3] ) ); out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "xlate" ) == 0 ) { mat_t mat; if ( nwords < 4 ) return(-1); /* Expects translations tx, ty, tz */ MAT_IDN( mat ); MAT_DELTAS( mat, atof( words[1] ), atof( words[2] ), atof( words[3] ) ); out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "rot_at" ) == 0 ) { mat_t mat; mat_t mat1; mat_t mat2; mat_t mat3; /* JG - Expects x, y, z, rx, ry, rz */ /* Translation back to the origin by (-x, -y, -z) */ /* is done first, then the rotation, and finally */ /* back into the original position by (+x, +y, +z). */ if ( nwords < 7 ) return(-1); MAT_IDN( mat1 ); MAT_IDN( mat2 ); MAT_IDN( mat3 ); MAT_DELTAS( mat1, -atof( words[1] ), -atof( words[2] ), -atof( words[3] ) ); bn_mat_angles( mat2, atof( words[4] ), atof( words[5] ), atof( words[6] ) ); MAT_DELTAS( mat3, atof( words[1] ), atof( words[2] ), atof( words[3] ) ); bn_mat_mul( mat, mat2, mat1 ); bn_mat_mul2( mat3, mat ); out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "orient" ) == 0 ) { register int i; mat_t mat; double args[8]; /* Expects tx, ty, tz, rx, ry, rz, [scale]. */ /* All rotation is done first, then translation */ /* Note: word[0] and args[0] are the keyword */ if ( nwords < 6+1 ) return(-1); for ( i=1; i<6+1; i++ ) args[i] = 0; args[7] = 1.0; /* optional arg, default to 1 */ for ( i=1; i<nwords; i++ ) args[i] = atof( words[i] ); MAT_IDN( mat ); bn_mat_angles( mat, args[4], args[5], args[6] ); MAT_DELTAS( mat, args[1], args[2], args[3] ); if ( NEAR_ZERO( args[7], VDIVIDE_TOL ) ) { /* Nearly zero, signal error */ fprintf(stderr, "Orient scale arg is near zero ('%s')\n", words[7] ); return(-1); } else { mat[15] = 1 / args[7]; } out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "ae" ) == 0 ) { mat_t mat; fastf_t az, el; if ( nwords < 3 ) return(-1); /* Expects azimuth, elev, optional twist */ az = atof(words[1]); el = atof(words[2]); #if 0 if ( nwords == 3 ) twist = 0.0; else twist = atof(words[3]); #endif MAT_IDN( mat ); /* XXX does not take twist, for now XXX */ bn_mat_ae( mat, az, el ); out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "arb_rot_pt" ) == 0 ) { mat_t mat; point_t pt1, pt2; vect_t dir; fastf_t ang; if ( nwords < 1+3+3+1 ) return(-1); /* Expects point1, point2, angle */ VSET( pt1, atof(words[1]), atof(words[2]), atof(words[3]) ); VSET( pt2, atof(words[4]), atof(words[5]), atof(words[6]) ); ang = atof(words[7]) * bn_degtorad; VSUB2( dir, pt2, pt2 ); VUNITIZE(dir); MAT_IDN( mat ); bn_mat_arb_rot( mat, pt1, dir, ang ); out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "arb_rot_dir" ) == 0 ) { mat_t mat; point_t pt1; vect_t dir; fastf_t ang; if ( nwords < 1+3+3+1 ) return(-1); /* Expects point1, dir, angle */ VSET( pt1, atof(words[1]), atof(words[2]), atof(words[3]) ); VSET( dir, atof(words[4]), atof(words[5]), atof(words[6]) ); ang = atof(words[7]) * bn_degtorad; VUNITIZE(dir); MAT_IDN( mat ); bn_mat_arb_rot( mat, pt1, dir, ang ); out_mat( mat, stdout ); return(0); } if ( strcmp( words[0], "quat" ) == 0 ) { mat_t mat; quat_t quat; /* Usage: quat x, y, z, w */ if ( nwords < 5 ) return -1; QSET( quat, atof(words[1]), atof(words[2]), atof(words[3]), atof(words[4]) ); quat_quat2mat( mat, quat ); out_mat( mat, stdout); return 0; } if ( strcmp( words[0], "fromto" ) == 0 ) { mat_t mat; point_t cur; point_t next; vect_t from; vect_t to; /* Usage: fromto +Z cur_xyz next_xyz */ if ( nwords < 8 ) return -1; if ( strcmp( words[1], "+X" ) == 0 ) { VSET( from, 1, 0, 0 ); } else if ( strcmp( words[1], "-X" ) == 0 ) { VSET( from, -1, 0, 0 ); } else if ( strcmp( words[1], "+Y" ) == 0 ) { VSET( from, 0, 1, 0 ); } else if ( strcmp( words[1], "-Y" ) == 0 ) { VSET( from, 0, -1, 0 ); } else if ( strcmp( words[1], "+Z" ) == 0 ) { VSET( from, 0, 0, 1 ); } else if ( strcmp( words[1], "-Z" ) == 0 ) { VSET( from, 0, 0, -1 ); } else { fprintf(stderr, "fromto '%s' is not +/-XYZ\n", words[1]); return -1; } VSET( cur, atof(words[2]), atof(words[3]), atof(words[4]) ); VSET( next, atof(words[5]), atof(words[6]), atof(words[7]) ); VSUB2( to, next, cur ); VUNITIZE(to); bn_mat_fromto( mat, from, to ); /* Check to see if it worked. */ { vect_t got; MAT4X3VEC( got, mat, from ); if ( VDOT( got, to ) < 0.9 ) { bu_log("\ntabsub ERROR: At t=%s, bn_mat_fromto failed!\n", chanwords[0] ); VPRINT("\tfrom", from); VPRINT("\tto", to); VPRINT("\tgot", got); } } out_mat( mat, stdout ); return 0; } return(-2); /* Unknown keyword */ }