libgender/util.C

/*
 * math support
 * most of the more complicated code is taken from mesa.
 */

#include <cstdio> // ugly
#include <cmath>

#include <sys/time.h>
#include <time.h>

#include "util.h"

#define DEG2RAD (M_PI / 180.)

const vec3 normalize (const vec3 &v)
{
  GLfloat s = abs (v);

  if (!s)
    return v;

  s = 1. / s;
  return vec3 (v.x * s, v.y * s, v.z * s);
}

const vec3 cross (const vec3 &a, const vec3 &b)
{
  return vec3 (
     a.y * b.z - a.z * b.y,
     a.z * b.x - a.x * b.z,
     a.x * b.y - a.y * b.x
  );
}

void gl_matrix::diagonal (GLfloat v)
{
  for (int i = 4; i--; )
    for (int j = 4; j--; )
      data[i][j] = i == j ? v : 0.;
}

const gl_matrix operator *(const gl_matrix &a, const gl_matrix &b)
{
  gl_matrix r;

  // taken from mesa
  for (int i = 0; i < 4; i++)
    {
      const GLfloat ai0=a(i,0), ai1=a(i,1), ai2=a(i,2), ai3=a(i,3);

      r(i,0) = ai0 * b(0,0) + ai1 * b(1,0) + ai2 * b(2,0) + ai3 * b(3,0);
      r(i,1) = ai0 * b(0,1) + ai1 * b(1,1) + ai2 * b(2,1) + ai3 * b(3,1);
      r(i,2) = ai0 * b(0,2) + ai1 * b(1,2) + ai2 * b(2,2) + ai3 * b(3,2);
      r(i,3) = ai0 * b(0,3) + ai1 * b(1,3) + ai2 * b(2,3) + ai3 * b(3,3);
    }

  return r;
}

void gl_matrix::rotate (GLfloat angle, const vec3 &axis)
{
  gl_matrix m;
  GLfloat xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;

  s = (GLfloat) sinf (angle * DEG2RAD);
  c = (GLfloat) cosf (angle * DEG2RAD);

  const GLfloat mag = abs (axis);

  if (mag <= 1.0e-4)
    return; /* no rotation, leave mat as-is */

  const vec3 n = axis * (1. / mag);

  /*
   *     Arbitrary axis rotation matrix.
   *
   *  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
   *  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
   *  (which is about the X-axis), and the two composite transforms
   *  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
   *  from the arbitrary axis to the X-axis then back.  They are
   *  all elementary rotations.
   *
   *  Rz' is a rotation about the Z-axis, to bring the axis vector
   *  into the x-z plane.  Then Ry' is applied, rotating about the
   *  Y-axis to bring the axis vector parallel with the X-axis.  The
   *  rotation about the X-axis is then performed.  Ry and Rz are
   *  simply the respective inverse transforms to bring the arbitrary
   *  axis back to it's original orientation.  The first transforms
   *  Rz' and Ry' are considered inverses, since the data from the
   *  arbitrary axis gives you info on how to get to it, not how
   *  to get away from it, and an inverse must be applied.
   *
   *  The basic calculation used is to recognize that the arbitrary
   *  axis vector (x, y, z), since it is of unit length, actually
   *  represents the sines and cosines of the angles to rotate the
   *  X-axis to the same orientation, with theta being the angle about
   *  Z and phi the angle about Y (in the order described above)
   *  as follows:
   *
   *  cos ( theta ) = x / sqrt ( 1 - z^2 )
   *  sin ( theta ) = y / sqrt ( 1 - z^2 )
   *
   *  cos ( phi ) = sqrt ( 1 - z^2 )
   *  sin ( phi ) = z
   *
   *  Note that cos ( phi ) can further be inserted to the above
   *  formulas:
   *
   *  cos ( theta ) = x / cos ( phi )
   *  sin ( theta ) = y / sin ( phi )
   *
   *  ...etc.  Because of those relations and the standard trigonometric
   *  relations, it is pssible to reduce the transforms down to what
   *  is used below.  It may be that any primary axis chosen will give the
   *  same results (modulo a sign convention) using this method.
   *
   *  Particularly nice is to notice that all divisions that might
   *  have caused trouble when parallel to certain planes or
   *  axis go away with care paid to reducing the expressions.
   *  After checking, it does perform correctly under all cases, since
   *  in all the cases of division where the denominator would have
   *  been zero, the numerator would have been zero as well, giving
   *  the expected result.
   */

  xx = n.x * n.x;
  yy = n.y * n.y;
  zz = n.z * n.z;
  xy = n.x * n.y;
  yz = n.y * n.z;
  zx = n.z * n.x;
  xs = n.x * s;
  ys = n.y * s;
  zs = n.z * s;
  one_c = 1.0F - c;

  m(0,0) = (one_c * xx) + c;
  m(0,1) = (one_c * xy) - zs;
  m(0,2) = (one_c * zx) + ys;
  m(0,3) = 0;
  
  m(1,0) = (one_c * xy) + zs;
  m(1,1) = (one_c * yy) + c;
  m(1,2) = (one_c * yz) - xs;
  m(1,3) = 0;
  
  m(2,0) = (one_c * zx) - ys;
  m(2,1) = (one_c * yz) + xs;
  m(2,2) = (one_c * zz) + c;
  m(2,3) = 0;
  
  m(3,0) = 0;
  m(3,1) = 0;
  m(3,2) = 0;
  m(3,3) = 1;

  (*this) = (*this) * m;
}

const vec3 operator *(const gl_matrix &a, const vec3 &v)
{
  return vec3 (
    a(0,0) * v.x + a(1,0) * v.y + a(2,0) * v.z + a(3,0),
    a(0,1) * v.x + a(1,1) * v.y + a(2,1) * v.z + a(3,1),
    a(0,2) * v.x + a(1,2) * v.y + a(2,2) * v.z + a(3,2)
  );
}

void gl_matrix::print ()
{
  printf ("\n");
  printf ("[ %f, %f, %f, %f ]\n", data[0][0], data[1][0], data[2][0], data[3][0]);
  printf ("[ %f, %f, %f, %f ]\n", data[0][1], data[1][1], data[2][1], data[3][1]);
  printf ("[ %f, %f, %f, %f ]\n", data[0][2], data[1][2], data[2][2], data[3][2]);
  printf ("[ %f, %f, %f, %f ]\n", data[0][3], data[1][3], data[2][3], data[3][3]);
}

void gl_matrix::translate (const vec3 &v)
{
  gl_matrix m(1);

  m(3,0) = v.x;
  m(3,1) = v.y;
  m(3,2) = v.z;
  m(3,3) = 1;

  (*this) = (*this) * m;
}

void box::add (const box &o)
{
  a.x = min (a.x, o.a.x);
  a.y = min (a.y, o.a.y);
  a.z = min (a.z, o.a.z);
  b.x = max (b.x, o.b.x);
  b.y = max (b.y, o.b.y);
  b.z = max (b.z, o.b.z);
}

void box::add (const point &p)
{
  a.x = min (a.x, p.x);
  a.y = min (a.y, p.y);
  a.z = min (a.z, p.z);
  b.x = max (b.x, p.x);
  b.y = max (b.y, p.y);
  b.z = max (b.z, p.z);
}

struct timer timer;
static double base;
double timer::now = 0.;
double timer::diff;

void timer::frame ()
{
  struct timeval tv;
  gettimeofday (&tv, 0);

  double next = tv.tv_sec - base + tv.tv_usec / 1.e6;

  diff = next - now;
  now = next;
}

timer::timer ()
{
  struct timeval tv;
  gettimeofday (&tv, 0);
  base = tv.tv_sec + tv.tv_usec / 1.e6;
}


Revision:	1.4
Committed:	Sun Oct 3 23:59:30 2004 UTC (19 years, 8 months ago) by root
Content type:	text/plain
Branch:	MAIN
Changes since 1.3:	+26 -4 lines
Log Message:	* empty log message *
#	User	Rev	Content
1	root	1.3	/*
2			* math support
3			* most of the more complicated code is taken from mesa.
4			*/
5
6	root	1.2	#include <cstdio> // ugly
7	root	1.1	#include <cmath>
8
9	root	1.4	#include <sys/time.h>
10			#include <time.h>
11	root	1.2
12	root	1.1	#include "util.h"
13
14	root	1.3	#define DEG2RAD (M_PI / 180.)
15
16	root	1.1	const vec3 normalize (const vec3 &v)
17			{
18	root	1.3	GLfloat s = abs (v);
19
20			if (!s)
21			return v;
22	root	1.1
23	root	1.3	s = 1. / s;
24	root	1.1	return vec3 (v.x * s, v.y * s, v.z * s);
25			}
26
27			const vec3 cross (const vec3 &a, const vec3 &b)
28			{
29			return vec3 (
30	root	1.3	a.y * b.z - a.z * b.y,
31			a.z * b.x - a.x * b.z,
32			a.x * b.y - a.y * b.x
33			);
34	root	1.2	}
35
36			void gl_matrix::diagonal (GLfloat v)
37			{
38			for (int i = 4; i--; )
39			for (int j = 4; j--; )
40			data[i][j] = i == j ? v : 0.;
41			}
42
43			const gl_matrix operator *(const gl_matrix &a, const gl_matrix &b)
44			{
45			gl_matrix r;
46
47	root	1.3	// taken from mesa
48			for (int i = 0; i < 4; i++)
49			{
50			const GLfloat ai0=a(i,0), ai1=a(i,1), ai2=a(i,2), ai3=a(i,3);
51
52			r(i,0) = ai0 * b(0,0) + ai1 * b(1,0) + ai2 * b(2,0) + ai3 * b(3,0);
53			r(i,1) = ai0 * b(0,1) + ai1 * b(1,1) + ai2 * b(2,1) + ai3 * b(3,1);
54			r(i,2) = ai0 * b(0,2) + ai1 * b(1,2) + ai2 * b(2,2) + ai3 * b(3,2);
55			r(i,3) = ai0 * b(0,3) + ai1 * b(1,3) + ai2 * b(2,3) + ai3 * b(3,3);
56			}
57	root	1.2
58	root	1.3	return r;
59			}
60	root	1.2
61	root	1.3	void gl_matrix::rotate (GLfloat angle, const vec3 &axis)
62			{
63			gl_matrix m;
64			GLfloat xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;
65	root	1.2
66	root	1.3	s = (GLfloat) sinf (angle * DEG2RAD);
67			c = (GLfloat) cosf (angle * DEG2RAD);
68
69			const GLfloat mag = abs (axis);
70
71			if (mag <= 1.0e-4)
72			return; /* no rotation, leave mat as-is */
73
74			const vec3 n = axis * (1. / mag);
75
76			/*
77			* Arbitrary axis rotation matrix.
78			*
79			* This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
80			* like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation
81			* (which is about the X-axis), and the two composite transforms
82			* Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
83			* from the arbitrary axis to the X-axis then back. They are
84			* all elementary rotations.
85			*
86			* Rz' is a rotation about the Z-axis, to bring the axis vector
87			* into the x-z plane. Then Ry' is applied, rotating about the
88			* Y-axis to bring the axis vector parallel with the X-axis. The
89			* rotation about the X-axis is then performed. Ry and Rz are
90			* simply the respective inverse transforms to bring the arbitrary
91			* axis back to it's original orientation. The first transforms
92			* Rz' and Ry' are considered inverses, since the data from the
93			* arbitrary axis gives you info on how to get to it, not how
94			* to get away from it, and an inverse must be applied.
95			*
96			* The basic calculation used is to recognize that the arbitrary
97			* axis vector (x, y, z), since it is of unit length, actually
98			* represents the sines and cosines of the angles to rotate the
99			* X-axis to the same orientation, with theta being the angle about
100			* Z and phi the angle about Y (in the order described above)
101			* as follows:
102			*
103			* cos ( theta ) = x / sqrt ( 1 - z^2 )
104			* sin ( theta ) = y / sqrt ( 1 - z^2 )
105			*
106			* cos ( phi ) = sqrt ( 1 - z^2 )
107			* sin ( phi ) = z
108			*
109			* Note that cos ( phi ) can further be inserted to the above
110			* formulas:
111			*
112			* cos ( theta ) = x / cos ( phi )
113			* sin ( theta ) = y / sin ( phi )
114			*
115			* ...etc. Because of those relations and the standard trigonometric
116			* relations, it is pssible to reduce the transforms down to what
117			* is used below. It may be that any primary axis chosen will give the
118			* same results (modulo a sign convention) using this method.
119			*
120			* Particularly nice is to notice that all divisions that might
121			* have caused trouble when parallel to certain planes or
122			* axis go away with care paid to reducing the expressions.
123			* After checking, it does perform correctly under all cases, since
124			* in all the cases of division where the denominator would have
125			* been zero, the numerator would have been zero as well, giving
126			* the expected result.
127			*/
128
129			xx = n.x * n.x;
130			yy = n.y * n.y;
131			zz = n.z * n.z;
132			xy = n.x * n.y;
133			yz = n.y * n.z;
134			zx = n.z * n.x;
135			xs = n.x * s;
136			ys = n.y * s;
137			zs = n.z * s;
138			one_c = 1.0F - c;
139
140			m(0,0) = (one_c * xx) + c;
141			m(0,1) = (one_c * xy) - zs;
142			m(0,2) = (one_c * zx) + ys;
143			m(0,3) = 0;
144
145			m(1,0) = (one_c * xy) + zs;
146			m(1,1) = (one_c * yy) + c;
147			m(1,2) = (one_c * yz) - xs;
148			m(1,3) = 0;
149
150			m(2,0) = (one_c * zx) - ys;
151			m(2,1) = (one_c * yz) + xs;
152			m(2,2) = (one_c * zz) + c;
153			m(2,3) = 0;
154
155			m(3,0) = 0;
156			m(3,1) = 0;
157			m(3,2) = 0;
158			m(3,3) = 1;
159
160			(this) = (this) * m;
161			}
162
163			const vec3 operator *(const gl_matrix &a, const vec3 &v)
164			{
165			return vec3 (
166			a(0,0) * v.x + a(1,0) * v.y + a(2,0) * v.z + a(3,0),
167			a(0,1) * v.x + a(1,1) * v.y + a(2,1) * v.z + a(3,1),
168			a(0,2) * v.x + a(1,2) * v.y + a(2,2) * v.z + a(3,2)
169			);
170	root	1.2	}
171
172			void gl_matrix::print ()
173			{
174			printf ("\n");
175			printf ("[ %f, %f, %f, %f ]\n", data[0][0], data[1][0], data[2][0], data[3][0]);
176			printf ("[ %f, %f, %f, %f ]\n", data[0][1], data[1][1], data[2][1], data[3][1]);
177			printf ("[ %f, %f, %f, %f ]\n", data[0][2], data[1][2], data[2][2], data[3][2]);
178			printf ("[ %f, %f, %f, %f ]\n", data[0][3], data[1][3], data[2][3], data[3][3]);
179			}
180
181			void gl_matrix::translate (const vec3 &v)
182			{
183			gl_matrix m(1);
184
185			m(3,0) = v.x;
186			m(3,1) = v.y;
187			m(3,2) = v.z;
188			m(3,3) = 1;
189
190			(this) = (this) * m;
191	root	1.1	}
192
193			void box::add (const box &o)
194			{
195			a.x = min (a.x, o.a.x);
196			a.y = min (a.y, o.a.y);
197			a.z = min (a.z, o.a.z);
198			b.x = max (b.x, o.b.x);
199			b.y = max (b.y, o.b.y);
200			b.z = max (b.z, o.b.z);
201			}
202
203			void box::add (const point &p)
204			{
205			a.x = min (a.x, p.x);
206			a.y = min (a.y, p.y);
207			a.z = min (a.z, p.z);
208			b.x = max (b.x, p.x);
209			b.y = max (b.y, p.y);
210			b.z = max (b.z, p.z);
211			}
212	root	1.4
213			struct timer timer;
214			static double base;
215			double timer::now = 0.;
216			double timer::diff;
217
218			void timer::frame ()
219			{
220			struct timeval tv;
221			gettimeofday (&tv, 0);
222
223			double next = tv.tv_sec - base + tv.tv_usec / 1.e6;
224
225			diff = next - now;
226			now = next;
227			}
228
229			timer::timer ()
230			{
231			struct timeval tv;
232			gettimeofday (&tv, 0);
233			base = tv.tv_sec + tv.tv_usec / 1.e6;
234			}
235
236	root	1.1