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.)

void renormalize (sector &s, point &p)
{
  float i;

  p.x = modff (p.x, &i); s.x += (int)i;
  p.y = modff (p.y, &i); s.y += (int)i;
  p.z = modff (p.z, &i); s.z += (int)i;
}

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

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