libgender/util.C

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

#include <cstdio> // ugly

#include <cmath>

#include <blitz/tinymat.h>

using namespace blitz;

#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);
}

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