Starshatter_Open
Open source Starshatter engine
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Geometry.cpp
Go to the documentation of this file.
1 /* Project nGenEx
2  Destroyer Studios LLC
3  Copyright © 1997-2004. All Rights Reserved.
4 
5  SUBSYSTEM: nGenEx.lib
6  FILE: Geometry.cpp
7  AUTHOR: John DiCamillo
8 
9 
10  OVERVIEW
11  ========
12  Geometric Utilities
13 */
14 
15 #include "MemDebug.h"
16 #include "Geometry.h"
17 
18 // +--------------------------------------------------------------------+
19 
20 void Rect::Inflate(int dx, int dy)
21 {
22  x -= dx;
23  w += dx*2;
24  y -= dy;
25  h += dy*2;
26 }
27 
28 void Rect::Deflate(int dx, int dy)
29 {
30  x += dx;
31  w -= dx*2;
32  y += dy;
33  h -= dy*2;
34 }
35 
36 void Rect::Inset(int l, int r, int t, int b)
37 {
38  x += l;
39  y += t;
40  w -= l + r;
41  h -= t + b;
42 }
43 
44 int Rect::Contains(int ax, int ay) const
45 {
46  if (ax < x) return 0;
47  if (ax > x+w) return 0;
48  if (ay < y) return 0;
49  if (ay > y+h) return 0;
50 
51  return 1;
52 }
53 
54 // +--------------------------------------------------------------------+
55 
56 double
57 Point::Normalize()
58 {
59  double scale = 1.0;
60  double len = length();
61 
62  if (len)
63  scale /= len;
64 
65  x *= scale;
66  y *= scale;
67  z *= scale;
68 
69  return len;
70 }
71 
72 // +--------------------------------------------------------------------+
73 
74 void
75 Point::SetElement(int i, double v)
76 {
77  switch (i) {
78  case 0: x = v; break;
79  case 1: y = v; break;
80  case 2: z = v; break;
81  default: break;
82  }
83 }
84 
85 // +--------------------------------------------------------------------+
86 
87 Point
88 Point::operator*(const Matrix& m) const
89 {
90  Point result;
91 
92  result.x = (m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z);
93  result.y = (m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z);
94  result.z = (m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z);
95 
96  return result;
97 }
98 
99 // +--------------------------------------------------------------------+
100 
101 double ClosestApproachTime(const Point& loc1, const Point& vel1,
102 const Point& loc2, const Point& vel2)
103 {
104  double t = 0;
105 
106  Point D = loc1-loc2;
107  Point Dv = vel1-vel2;
108 
109  if (Dv.x || Dv.y || Dv.z)
110  t = -1 * (Dv*D) / (Dv*Dv);
111 
112  return t;
113 }
114 
115 // +--------------------------------------------------------------------+
116 
117 float
118 Vec2::Normalize()
119 {
120  float scale = 1.0f;
121  float len = length();
122 
123  if (len)
124  scale /= len;
125 
126  x *= scale;
127  y *= scale;
128 
129  return len;
130 }
131 
132 // +--------------------------------------------------------------------+
133 
134 float
135 Vec3::Normalize()
136 {
137  float scale = 1.0f;
138  float len = length();
139 
140  if (len)
141  scale /= len;
142 
143  x *= scale;
144  y *= scale;
145  z *= scale;
146 
147  return len;
148 }
149 
150 // +--------------------------------------------------------------------+
151 
152 Vec3
153 Vec3::operator*(const Matrix& m) const
154 {
155  Vec3 result;
156 
157  result.x = (float) ((m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z));
158  result.y = (float) ((m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z));
159  result.z = (float) ((m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z));
160 
161  return result;
162 }
163 
164 // +--------------------------------------------------------------------+
165 
166 double ClosestApproachTime(const Vec3& loc1, const Vec3& vel1,
167 const Vec3& loc2, const Vec3& vel2)
168 {
169  double t = 0;
170 
171  Point D = loc1-loc2;
172  Point Dv = vel1-vel2;
173 
174  if (Dv.x || Dv.y || Dv.z)
175  t = -1 * (Dv*D) / (Dv*Dv);
176 
177  return t;
178 }
179 
180 // +--------------------------------------------------------------------+
181 
182 double
183 Quaternion::Normalize()
184 {
185  double scale = 1.0;
186  double len = length();
187 
188  if (len)
189  scale /= len;
190 
191  x *= scale;
192  y *= scale;
193  z *= scale;
194  w *= scale;
195 
196  return len;
197 }
198 
199 // +--------------------------------------------------------------------+
200 
201 Matrix::Matrix()
202 {
203  Identity();
204 }
205 
206 Matrix::Matrix(const Matrix& m)
207 {
208  CopyMemory(elem, m.elem, sizeof(elem));
209 }
210 
211 Matrix::Matrix(const Point& vrt, const Point& vup, const Point& vpn)
212 {
213  elem[0][0] = vrt.x;
214  elem[0][1] = vrt.y;
215  elem[0][2] = vrt.z;
216 
217  elem[1][0] = vup.x;
218  elem[1][1] = vup.y;
219  elem[1][2] = vup.z;
220 
221  elem[2][0] = vpn.x;
222  elem[2][1] = vpn.y;
223  elem[2][2] = vpn.z;
224 }
225 
226 // +--------------------------------------------------------------------+
227 
228 Matrix&
229 Matrix::operator =(const Matrix& m)
230 {
231  CopyMemory(elem, m.elem, sizeof(elem));
232 
233  return *this;
234 }
235 
236 // +--------------------------------------------------------------------+
237 
238 Matrix&
239 Matrix::operator*=(const Matrix& m)
240 {
241  return *this = *this * m;
242 }
243 
244 // +--------------------------------------------------------------------+
245 
246 void
247 Matrix::Identity()
248 {
249  elem[0][0] = 1;
250  elem[0][1] = 0;
251  elem[0][2] = 0;
252 
253  elem[1][0] = 0;
254  elem[1][1] = 1;
255  elem[1][2] = 0;
256 
257  elem[2][0] = 0;
258  elem[2][1] = 0;
259  elem[2][2] = 1;
260 }
261 
262 // +--------------------------------------------------------------------+
263 
264 inline void swap_elem(double& a, double& b) { double t=a; a=b; b=t; }
265 
266 void
267 Matrix::Transpose()
268 {
269  swap_elem(elem[0][1], elem[1][0]);
270  swap_elem(elem[0][2], elem[2][0]);
271  swap_elem(elem[1][2], elem[2][1]);
272 }
273 
274 // +--------------------------------------------------------------------+
275 
276 void
277 Matrix::Rotate(double roll, double pitch, double yaw)
278 {
279  double e[3][3];
280  CopyMemory(e, elem, sizeof(elem));
281 
282  double sr = sin(roll);
283  double cr = cos(roll);
284  double sp = sin(pitch);
285  double cp = cos(pitch);
286  double sy = sin(yaw);
287  double cy = cos(yaw);
288 
289  double a,b,c;
290 
291  a = cy*cr;
292  b = cy*sr;
293  c = -sy;
294 
295  elem[0][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
296  elem[0][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
297  elem[0][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
298 
299  a = cp*-sr + sp*sy*cr;
300  b = cp* cr + sp*sy*sr;
301  c = sp*cy;
302 
303  elem[1][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
304  elem[1][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
305  elem[1][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
306 
307  a = -sp*-sr + cp*sy*cr;
308  b = -sp* cr + cp*sy*sr;
309  c = cp*cy;
310 
311  elem[2][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
312  elem[2][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
313  elem[2][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
314 }
315 
316 // +--------------------------------------------------------------------+
317 
318 void
319 Matrix::Roll(double roll)
320 {
321  double s = sin(roll);
322  double c = cos(roll);
323 
324  double e00 = elem[0][0];
325  double e01 = elem[0][1];
326  double e02 = elem[0][2];
327  double e10 = elem[1][0];
328  double e11 = elem[1][1];
329  double e12 = elem[1][2];
330 
331  elem[0][0] = c*e00 + s*e10;
332  elem[0][1] = c*e01 + s*e11;
333  elem[0][2] = c*e02 + s*e12;
334 
335  elem[1][0] = -s*e00 + c*e10;
336  elem[1][1] = -s*e01 + c*e11;
337  elem[1][2] = -s*e02 + c*e12;
338 }
339 
340 // +--------------------------------------------------------------------+
341 
342 void
343 Matrix::Pitch(double pitch)
344 {
345  double s = sin(pitch);
346  double c = cos(pitch);
347 
348  double e10 = elem[1][0];
349  double e11 = elem[1][1];
350  double e12 = elem[1][2];
351  double e20 = elem[2][0];
352  double e21 = elem[2][1];
353  double e22 = elem[2][2];
354 
355  elem[1][0] = c*e10 + s*e20;
356  elem[1][1] = c*e11 + s*e21;
357  elem[1][2] = c*e12 + s*e22;
358 
359  elem[2][0] = -s*e10 + c*e20;
360  elem[2][1] = -s*e11 + c*e21;
361  elem[2][2] = -s*e12 + c*e22;
362 }
363 
364 // +--------------------------------------------------------------------+
365 
366 void
367 Matrix::Yaw(double yaw)
368 {
369  double s = sin(yaw);
370  double c = cos(yaw);
371 
372  double e00 = elem[0][0];
373  double e01 = elem[0][1];
374  double e02 = elem[0][2];
375  double e20 = elem[2][0];
376  double e21 = elem[2][1];
377  double e22 = elem[2][2];
378 
379  elem[0][0] = c*e00 - s*e20;
380  elem[0][1] = c*e01 - s*e21;
381  elem[0][2] = c*e02 - s*e22;
382 
383  elem[2][0] = s*e00 + c*e20;
384  elem[2][1] = s*e01 + c*e21;
385  elem[2][2] = s*e02 + c*e22;
386 }
387 
388 // +--------------------------------------------------------------------+
389 
390 inline int sign(double d) { return (d >= 0); }
391 
392 void
393 Matrix::ComputeEulerAngles(double& roll, double& pitch, double& yaw) const
394 {
395  double cy;
396 
397  yaw = asin(-elem[0][2]);
398  cy = cos(yaw);
399  roll = asin(elem[0][1] / cy);
400  pitch = asin(elem[1][2] / cy);
401 
402  if (sign(cos(roll)*cy) != sign(elem[0][0]))
403  roll = PI - roll;
404 
405  if (sign(cos(pitch)*cy) != sign(elem[2][2]))
406  pitch = PI - pitch;
407 }
408 
409 // +--------------------------------------------------------------------+
410 
411 Matrix
412 Matrix::operator*(const Matrix& m) const
413 {
414  Matrix r;
415 
416  r.elem[0][0] = elem[0][0]*m.elem[0][0] + elem[0][1]*m.elem[1][0] + elem[0][2]*m.elem[2][0];
417  r.elem[0][1] = elem[0][0]*m.elem[0][1] + elem[0][1]*m.elem[1][1] + elem[0][2]*m.elem[2][1];
418  r.elem[0][2] = elem[0][0]*m.elem[0][2] + elem[0][1]*m.elem[1][2] + elem[0][2]*m.elem[2][2];
419 
420  r.elem[1][0] = elem[1][0]*m.elem[0][0] + elem[1][1]*m.elem[1][0] + elem[1][2]*m.elem[2][0];
421  r.elem[1][1] = elem[1][0]*m.elem[0][1] + elem[1][1]*m.elem[1][1] + elem[1][2]*m.elem[2][1];
422  r.elem[1][2] = elem[1][0]*m.elem[0][2] + elem[1][1]*m.elem[1][2] + elem[1][2]*m.elem[2][2];
423 
424  r.elem[2][0] = elem[2][0]*m.elem[0][0] + elem[2][1]*m.elem[1][0] + elem[2][2]*m.elem[2][0];
425  r.elem[2][1] = elem[2][0]*m.elem[0][1] + elem[2][1]*m.elem[1][1] + elem[2][2]*m.elem[2][1];
426  r.elem[2][2] = elem[2][0]*m.elem[0][2] + elem[2][1]*m.elem[1][2] + elem[2][2]*m.elem[2][2];
427 
428  return r;
429 }
430 
431 // +--------------------------------------------------------------------+
432 
433 Point
434 Matrix::operator*(const Point& p) const
435 {
436  Point result;
437 
438  result.x = (elem[0][0] * p.x) + (elem[0][1] * p.y) + (elem[0][2] * p.z);
439  result.y = (elem[1][0] * p.x) + (elem[1][1] * p.y) + (elem[1][2] * p.z);
440  result.z = (elem[2][0] * p.x) + (elem[2][1] * p.y) + (elem[2][2] * p.z);
441 
442  return result;
443 }
444 
445 // +--------------------------------------------------------------------+
446 
447 Vec3
448 Matrix::operator*(const Vec3& v) const
449 {
450  Vec3 result;
451 
452  result.x = (float) ((elem[0][0] * v.x) + (elem[0][1] * v.y) + (elem[0][2] * v.z));
453  result.y = (float) ((elem[1][0] * v.x) + (elem[1][1] * v.y) + (elem[1][2] * v.z));
454  result.z = (float) ((elem[2][0] * v.x) + (elem[2][1] * v.y) + (elem[2][2] * v.z));
455 
456  return result;
457 }
458 
459 // +--------------------------------------------------------------------+
460 
461 double
462 Matrix::Cofactor(int i, int j) const
463 {
464  int i1=0;
465  int i2=2;
466  int j1=0;
467  int j2=2;
468 
469  if (i==0) i1=1; else if (i==2) i2=1;
470  if (j==0) j1=1; else if (j==2) j2=1;
471 
472  double factor = elem[i1][j1]*elem[i2][j2] - elem[i1][j2]*elem[i2][j1];
473 
474  if ((i+j) & 1)
475  factor *= -1;
476 
477  return factor;
478 }
479 
480 // +--------------------------------------------------------------------+
481 
482 void
483 Matrix::Invert()
484 {
485  double f[3][3];
486  int i, j;
487 
488  for (i = 0; i < 3; i++)
489  for (j = 0; j < 3; j++)
490  f[i][j] = Cofactor(j,i);
491 
492  double det = elem[0][0] * f[0][0] +
493  elem[0][1] * f[1][0] +
494  elem[0][2] * f[2][0];
495 
496  if (det != 0) {
497  double inv = 1/det;
498 
499  for (i = 0; i < 3; i++)
500  for (j = 0; j < 3; j++)
501  elem[i][j] = f[i][j] * inv;
502  }
503 }
504 
505 // +--------------------------------------------------------------------+
506 // +--------------------------------------------------------------------+
507 // +--------------------------------------------------------------------+
508 
509 Plane::Plane()
510 : distance(0.0f)
511 { }
512 
513 Plane::Plane(const Point& p0, const Point& p1, const Point& p2)
514 {
515  Point d1 = p1 - p0;
516  Point d2 = p2 - p0;
517 
518  normal = (Vec3) d1.cross(d2);
519  normal.Normalize();
520 
521  distance = (float) (normal * p0);
522 }
523 
524 Plane::Plane(const Vec3& v0, const Vec3& v1, const Vec3& v2)
525 {
526  Vec3 d1 = v1 - v0;
527  Vec3 d2 = v2 - v0;
528 
529  normal = d1.cross(d2);
530  normal.Normalize();
531 
532  distance = normal * v0;
533 }
534 
535 void Plane::Rotate(const Vec3& v0, const Matrix& m)
536 {
537  normal = normal * m;
538  distance = normal * v0;
539 }
540 
541 void Plane::Translate(const Vec3& v0)
542 {
543  distance = normal * v0;
544 }
545 
546 // +--------------------------------------------------------------------+
547 // 3-D dot product.
548 
549 double DotProduct(const Point& a, const Point& b)
550 {
551  return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
552 }
553 
554 // +--------------------------------------------------------------------+
555 // 3-D cross product.
556 
557 void CrossProduct(const Point& a, const Point& b, Point& out)
558 {
559  out.x = (a.y * b.z) - (a.z * b.y);
560  out.y = (a.z * b.x) - (a.x * b.z);
561  out.z = (a.x * b.y) - (a.y * b.x);
562 }
563 
564 // +--------------------------------------------------------------------+
565 // Concatenate two 3x3 matrices.
566 
567 void MConcat(double in1[3][3], double in2[3][3], double out[3][3])
568 {
569  int i, j;
570 
571  for (i=0 ; i<3 ; i++) {
572  for (j=0 ; j<3 ; j++) {
573  out[i][j] = in1[i][0] * in2[0][j] +
574  in1[i][1] * in2[1][j] +
575  in1[i][2] * in2[2][j];
576  }
577  }
578 }
579 
580 /* GRAPHICS GEMS II ----------------------------------------------------
581 *
582 * lines_intersect: AUTHOR: Mukesh Prasad
583 *
584 * This function computes whether two line segments,
585 * respectively joining the input points (x1,y1) -- (x2,y2)
586 * and the input points (x3,y3) -- (x4,y4) intersect.
587 * If the lines intersect, the output variables x, y are
588 * set to coordinates of the point of intersection.
589 *
590 * All values are in integers. The returned value is rounded
591 * to the nearest integer point.
592 *
593 * If non-integral grid points are relevant, the function
594 * can easily be transformed by substituting floating point
595 * calculations instead of integer calculations.
596 *
597 * Entry
598 * x1, y1, x2, y2 Coordinates of endpoints of one segment.
599 * x3, y3, x4, y4 Coordinates of endpoints of other segment.
600 *
601 * Exit
602 * x, y Coordinates of intersection point.
603 *
604 * The value returned by the function is one of:
605 *
606 * DONT_INTERSECT 0
607 * DO_INTERSECT 1
608 * COLLINEAR 2
609 *
610 * Error conditions:
611 *
612 * Depending upon the possible ranges, and particularly on 16-bit
613 * computers, care should be taken to protect from overflow.
614 *
615 * In the following code, 'long' values have been used for this
616 * purpose, instead of 'int'.
617 *
618 */
619 
620 #define DONT_INTERSECT 0
621 #define DO_INTERSECT 1
622 #define COLLINEAR 2
623 
624 inline int SAME_SIGNS(double a, double b)
625 {
626  return ((a>=0 && b>=0) || (a<0 && b<0));
627 }
628 
629 int
630 lines_intersect(
631 /* 1st line segment */ double x1, double y1, double x2, double y2,
632 /* 2nd line segment */ double x3, double y3, double x4, double y4,
633 /* pt of intersect */ double& ix, double& iy)
634 {
635  double a1, a2, b1, b2, c1, c2; /* Coefficients of line eqns. */
636  double r1, r2, r3, r4; /* 'Sign' values */
637  double denom, offset, num; /* Intermediate values */
638 
639  /* Compute a1, b1, c1, where line joining points 1 and 2
640  * is "a1 x + b1 y + c1 = 0". */
641 
642  a1 = y2 - y1;
643  b1 = x1 - x2;
644  c1 = x2 * y1 - x1 * y2;
645 
646  /* Compute r3 and r4. */
647 
648  r3 = a1 * x3 + b1 * y3 + c1;
649  r4 = a1 * x4 + b1 * y4 + c1;
650 
651  /* Check signs of r3 and r4. If both point 3 and point 4 lie on
652  * same side of line 1, the line segments do not intersect. */
653 
654  if ( r3 != 0 &&
655  r4 != 0 &&
656  SAME_SIGNS( r3, r4 ))
657  return ( DONT_INTERSECT );
658 
659  /* Compute a2, b2, c2 */
660 
661  a2 = y4 - y3;
662  b2 = x3 - x4;
663  c2 = x4 * y3 - x3 * y4;
664 
665  /* Compute r1 and r2 */
666 
667  r1 = a2 * x1 + b2 * y1 + c2;
668  r2 = a2 * x2 + b2 * y2 + c2;
669 
670  /* Check signs of r1 and r2. If both point 1 and point 2 lie
671  * on same side of second line segment, the line segments do
672  * not intersect. */
673 
674  if ( r1 != 0 &&
675  r2 != 0 &&
676  SAME_SIGNS( r1, r2 ))
677  return ( DONT_INTERSECT );
678 
679  /* Line segments intersect: compute intersection point. */
680 
681  denom = a1 * b2 - a2 * b1;
682  if ( denom == 0 )
683  return ( DONT_INTERSECT );
684  offset = denom < 0 ? - denom / 2 : denom / 2;
685 
686  /* The denom/2 is to get rounding instead of truncating. It
687  * is added or subtracted to the numerator, depending upon the
688  * sign of the numerator. */
689 
690  num = b1 * c2 - b2 * c1;
691  ix = ( num < 0 ? num - offset : num + offset ) / denom;
692 
693  num = a2 * c1 - a1 * c2;
694  iy = ( num < 0 ? num - offset : num + offset ) / denom;
695 
696  return ( DO_INTERSECT );
697 }
698