Skip to content
Permalink
master
Switch branches/tags
Go to file
 
 
Cannot retrieve contributors at this time
#include <math.h> #define EPSILON 0.000001
#define CROSS(dest, v1, v2){ \
dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; \
dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; \
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];}
#define DOT(v1, v2) (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] *
v2[2])
#define SUB(dest, v1, v2){ \
dest[0] = v1[0] - v2[0]; \
dest[1] = v1[1] - v2[1]; \
dest[2] = v1[2] - v2[2];}
/*
* A function for creating a rotation matrix that rotates a vector called
* "from" into another vector called "to".
* Input : from[3], to[3] which both must be *normalized* non-zero vectors
* Output: mtx[3][3] -- a 3x3 matrix in colum-major form
* Authors: Tomas Möller, John Hughes 1999
*/
void fromToRotation(float from[3], float to[3], float mtx[3][3]) {
float v[3];
float e, h, f;
CROSS(v, from, to);
e = DOT(from, to);
f = (e < 0)? -e:e;
if (f > 1.0 - EPSILON) /* "from" and "to"-vector almost parallel */
{
float u[3], v[3]; /* temporary storage vectors */
float x[3]; /* vector most nearly orthogonal to "from" */
float c1, c2, c3; /* coefficients for later use */
int i, j;
x[0] = (from[0] > 0.0)? from[0] : -from[0];
x[1] = (from[1] > 0.0)? from[1] : -from[1];
x[2] = (from[2] > 0.0)? from[2] : -from[2];
if (x[0] < x[1])
{
if (x[0] < x[2])
{
x[0] = 1.0; x[1] = x[2] = 0.0;
}
else
{
x[2] = 1.0; x[0] = x[1] = 0.0;
}
}
else
{
if (x[1] < x[2])
{
x[1] = 1.0; x[0] = x[2] = 0.0;
}
else
{
x[2] = 1.0; x[0] = x[1] = 0.0;
}
}
u[0] = x[0] - from[0]; u[1] = x[1] - from[1]; u[2] = x[2] - from[2];
v[0] = x[0] - to[0]; v[1] = x[1] - to[1]; v[2] = x[2] - to[2];
c1 = 2.0 / DOT(u, u);
c2 = 2.0 / DOT(v, v);
c3 = c1 * c2 * DOT(u, v);
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
mtx[i][j] = - c1 * u[i] * u[j]
- c2 * v[i] * v[j]
+ c3 * v[i] * u[j];
}
mtx[i][i] += 1.0;
}
}
else /* the most common case, unless "from"="to", or "from"=-"to" */
{
#if 0
/* unoptimized version - a good compiler will optimize this. */
/* h = (1.0 - e)/DOT(v, v); old code */
h = 1.0/(1.0 + e); /* optimization by Gottfried Chen */
mtx[0][0] = e + h * v[0] * v[0];
mtx[0][1] = h * v[0] * v[1] - v[2];
mtx[0][2] = h * v[0] * v[2] + v[1];
mtx[1][0] = h * v[0] * v[1] + v[2];
mtx[1][1] = e + h * v[1] * v[1];
mtx[1][2] = h * v[1] * v[2] - v[0];
mtx[2][0] = h * v[0] * v[2] - v[1];
mtx[2][1] = h * v[1] * v[2] + v[0];
mtx[2][2] = e + h * v[2] * v[2];
#else
/* ...otherwise use this hand optimized version (9 mults less) */
float hvx, hvz, hvxy, hvxz, hvyz;
/* h = (1.0 - e)/DOT(v, v); old code */
h = 1.0/(1.0 + e); /* optimization by Gottfried Chen */
hvx = h * v[0];
hvz = h * v[2];
hvxy = hvx * v[1];
hvxz = hvx * v[2];
hvyz = hvz * v[1];
mtx[0][0] = e + hvx * v[0];
mtx[0][1] = hvxy - v[2];
mtx[0][2] = hvxz + v[1];
mtx[1][0] = hvxy + v[2];
mtx[1][1] = e + h * v[1] * v[1];
mtx[1][2] = hvyz - v[0];
mtx[2][0] = hvxz - v[1];
mtx[2][1] = hvyz + v[0];
mtx[2][2] = e + hvz * v[2];
#endif
}
}