|
And by the matrix I mean this crap:
TQ3Matrix4x4 * E3Matrix4x4_SetRotateVectorToVector(TQ3Matrix4x4 *matrix4x4, const TQ3Vector3D *v1, const TQ3Vector3D *v2) { // We accomplish the rotation by creating two orthonormal vector triads: // // (u, v, w) -> (u', v', w) // // The rotation is about the axis w. It rotates u (=v1) into u' (=v2). It // also rotates v into v', which are vectors rotated 90 degrees in the plane // of rotation from u and u', respectively. // // To construct the rotation matrix, we rotate into and out of the basis // vectors i, j, k: // // (u, v, w) -> (i, j, k) -> (u', v', w) // // Thus the rotation matrix is the product of two rotation matrices, a and b: // // | ux vx wx | | ux' uy' uz' | // | uy vy wy | . | vx' vy' vz' | // | uz vz wz | | wx wy wz | // // To see this, simply multiply this composite matrix by the u, v or w row // vector on the left and see that the result is u', v' or w, respectively. TQ3Vector3D u, uPrime, v, vPrime, w; TQ3Matrix3x3 a, b;
// Construct vector w (axis of rotation) perpendicular to v1 and v2 Q3Vector3D_Cross(v1, v2, &w); // Check if vector w is roughly zero if (e3vector3d_below_tolerance(&w, 10.0f*FLT_EPSILON)) { // Vectors v1 and v2 are approximately parallel or approximately anti-parallel // (within 1.192092896e-07 radians or roughly 0.000007 degrees!)
TQ3Vector3D v2Proxy; TQ3Int32 iSmall, i; float valueSmall; // Determine v1 component of smallest absolute value iSmall = 0; valueSmall = (float) fabs(v1->x); for (i = 1; i < 3; ++i) { float value; value = (float) fabs(((const float*) (v1))); if (value < valueSmall) { iSmall = i; valueSmall = value; } } // Construct corresponding basis vector for (i = 0; i < 3; ++i) ((float*) (&v2Proxy)) = (i == iSmall ? 1.0f : 0.0f); // Construct vector w (axis of rotation) perpendicular to v1 and v2Proxy Q3Vector3D_Cross(v1, &v2Proxy, &w); } Q3Vector3D_Normalize(&w, &w); u = *v1; uPrime = *v2; Q3Vector3D_Cross(&w, &u, &v); Q3Vector3D_Cross(&w, &uPrime, &vPrime);
#define A(x,y) a.value #define B(x,y) b.value #define M(x,y) matrix4x4->value A(0,0) = u.x; A(0,1) = v.x; A(0,2) = w.x;
A(1,0) = u.y; A(1,1) = v.y; A(1,2) = w.y; A(2,0) = u.z; A(2,1) = v.z; A(2,2) = w.z; B(0,0) = uPrime.x; B(0,1) = uPrime.y; B(0,2) = uPrime.z;
B(1,0) = vPrime.x; B(1,1) = vPrime.y; B(1,2) = vPrime.z; B(2,0) = w.x; B(2,1) = w.y; B(2,2) = w.z; // Now multiply the two 3x3 matrices a and b to get the 3x3 part of the result, // filling out the rest of the result as an identity matrix (since we are // rotating about the point <0,0,0>) M(0,0) = A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0); M(0,1) = A(0,0)*B(0,1) + A(0,1)*B(1,1) + A(0,2)*B(2,1); M(0,2) = A(0,0)*B(0,2) + A(0,1)*B(1,2) + A(0,2)*B(2,2); M(0,3) = 0.0f; M(1,0) = A(1,0)*B(0,0) + A(1,1)*B(1,0) + A(1,2)*B(2,0); M(1,1) = A(1,0)*B(0,1) + A(1,1)*B(1,1) + A(1,2)*B(2,1); M(1,2) = A(1,0)*B(0,2) + A(1,1)*B(1,2) + A(1,2)*B(2,2); M(1,3) = 0.0f; M(2,0) = A(2,0)*B(0,0) + A(2,1)*B(1,0) + A(2,2)*B(2,0); M(2,1) = A(2,0)*B(0,1) + A(2,1)*B(1,1) + A(2,2)*B(2,1); M(2,2) = A(2,0)*B(0,2) + A(2,1)*B(1,2) + A(2,2)*B(2,2); M(2,3) = 0.0f; M(3,0) = 0.0f; M(3,1) = 0.0f; M(3,2) = 0.0f; M(3,3) = 1.0f;
#undef A #undef B #undef M
return(matrix4x4); }
x( :banghead:
|